Control device for mobile body

ABSTRACT

Ground contact portions  10  are classified into a tree structure such that each of the ground contact portions  10  of a mobile body  1  (mobile robot) equipped with three or more ground contact portions  10  becomes a leaf node and that an intermediate node exists between the leaf node and a root node having all the leaf nodes as its descendant nodes. On each node (a C-th node) having child nodes, the correction amounts of the desired relative heights of the ground contact portions  10  of the C-th node are determined such that the relative relationship among the actual node floor reaction forces of the child nodes of the C-th node approximates the relative relationship among the desired node floor reaction forces of the child nodes of the C-th node, and joints of the mobile body  1  are operated so that a desired relative height obtained by combining the correction amounts is satisfied.

TECHNICAL FIELD

The present invention relates to the control of a floor reaction forceof a mobile body, such as a legged mobile robot and a wheeled mobilebody having an active suspension. More specifically, the presentinvention relates to a device that is resistant to the influences of anunknown slope, projection or depression of a floor (ground surface) withwhich a mobile body, such as the legged mobile robot or the wheeledmobile robot having an active suspension, comes in contact, and thatproperly controls floor reaction forces. Moreover, the present inventionrelates to a device adapted to properly control a floor reaction forcewhile estimating the configuration of a floor.

BACKGROUND ART

Japanese Unexamined Patent Application Publication No. H5-305585 by thepresent applicant (Patent Document 1) discloses a technology of thecompliance control of both legs of a two-legged mobile robot. Thistechnology is applicable also to a robot having, for example, four legs.

In this case, according to the compliance control based on thetechnology disclosed in the publication, a virtual plane is inclinedwhile all legs are in contact with the virtual plane, so that anundulation or tilt over a large area of the floor can be coped with.According to the technology of the publication, however, there have beencases where a load (floor reaction force) is not properly distributed ifa floor has a local tilt, projection or depression. As a result, poorcontact with a surface has sometimes occurred, causing a portion (foot)to be in contact therewith to shake or to be subjected to an unduelanding impact, with consequent deteriorated properties of foot contactwith the ground. This has further led to a danger in that a robot easilyslips or spins. Further, there have been cases where control fails toproperly distribute a floor reaction force moment for restoring theposture of the robot to a desired posture to the distal portion of eachleg, causing the posture of the robot to shake. There have been alsocases where, if a certain portion of the robot starts vibrating, then itis inconveniently difficult to attenuate the vibration.

Further, there have been cases where the failure of proper control ofdistribution of a floor reaction force to the distal end of each legresults in excessive load on some actuators.

In addition, Japanese Unexamined Patent Application Publication No.H10-277969 (Patent Document 2) and Japanese Unexamined PatentApplication Publication No. 2001-322076 (Patent Document 3) by thepresent applicant disclose a technology for composite-compliance controland a technology for estimating the configuration of a floor.

These technologies make it possible to properly distribute a floorreaction force to the distal portion of each leg in a two-legged mobilerobot and to compositively estimate the configuration of a floor, morespecifically, compositively and accurately estimate the inclination ofthe ground contact surface of each leg distal portion and the heightdifference between ground contact surfaces at the same time. Thesetechnologies in the publications are, in principle, applicable to amulti-legged robot, such as a four-legged robot; however, no descriptionhas been given of a technology for specifically applying them.

Further, there has been known a technique for estimating the height of afloor at the time of landing by using feelers (load sensors or contactsensors) provided at the distal ends of the legs of the four-leggedrobot.

However, this technique is a sequential technique and has been incapableof dealing with a case where a floor deforms after landing, as with asoft ground. Especially when the main body of the robot is severelyvibrating, such as when the robot is moving fast, the height of a floorcannot be correctly estimated, and there has been a danger in that loadis not properly distributed to each leg.

There has been also known a technology whereby the compliance control ofeach leg of a robot is independently conducted for each leg. Thistechnology, however, does not take into account the fact that the legsinterfere with each other through the vibration of the body of therobot. In other words, the compliance control is individually carriedout on each leg. For this reason, the compliance control of each legdoes not act in harmony, making it impossible to properly distribute afloor reaction force to each leg.

The present invention has been made with a view of the backgrounddescribed above, and it is an object thereof to provide a device that isresistant to the influences of an unknown slope, projection ordepression of a floor with which a mobile body, such as a legged mobilerobot or a wheeled mobile body having an active suspension, comes incontact, and that properly controls a floor reaction force. Moreover, anobject of the present invention is to provide a device capable ofproperly controlling a floor reaction force while accurately estimatingthe configuration of a floor.

DISCLOSURE OF INVENTION

According to a first invention of a control device for a mobile body ofthe present invention, a control device for a mobile body that comprisesa desired gait determining means for determining a desired gait composedof a desired motion and a desired floor reaction force of a mobile body,such as a mobile robot having three or more ground contact portionsconnected through the intermediary of a plurality of joints such thatmutual relative heights thereof can be manipulated, and a floor reactionforce detecting means for detecting or estimating an actual floorreaction force acting on each of the ground contact portions, andcontrols the operation of the mobile body to make a gait of the mobilebody follow a desired gait,

wherein the ground contact portions are classified into a tree structuresuch that each of the ground contact portions becomes a leaf node, andan intermediate node exists between the leaf node and a root node havingall the leaf nodes as descendant nodes, and

relative to a B-th node, which is each node in the tree structure, ifthe B-th node is the leaf node, then a floor reaction force acting on aground contact portion that is the leaf node is defined as the nodefloor reaction force of the B-th node, and if the B-th node has childnodes, then the resultant force of the node floor reaction forces of allchild nodes of the B-th node is defined as the node floor reaction forceof the B-th node,

a control device for a mobile body, comprises:

a means for determining an actual node floor reaction force as theactual value of a node floor reaction force of each node from thedetected or estimated actual floor reaction force of each ground contactportion;

a means for determining a desired node floor reaction force as thedesired value of a node floor reaction force of each node on the basisof at least a desired floor reaction force of the desired gait; and

a node operation controlling means for determining, on each of C-thnodes, which is a node having a plurality of ground contact portions asdescendant nodes, the correction amounts of the desired relative heightsof a plurality of ground contact portions belonging to the C-th node onthe basis of at least the relative relationship among the actual nodefloor reaction forces of the child nodes of the C-th node and therelative relationship among the desired node floor reaction forces ofthe child nodes of the C-th node, determines a corrected desired motionby adding at least a first correction of the desired relative heights ofthe plurality of ground contact portions belonging to each C-th node toa desired motion of the desired gait on the basis of a correction amountobtained by combining the determined correction amount of every C-thnode, and operates the joints such that the corrected desired motionthat has been determined is satisfied.

In the present invention, regarding each node having child nodes,“ground contact portions belonging to the node” means, more accurately,“belonging as descendant nodes to the node.” This applies not only tothe first invention but also applies to any one of the inventions of thepresent application.

According to the first invention, three or more ground contact portionsprovided in a mobile body are classified into a tree structure, and thecorrection amounts (in other words, manipulated variables) of thedesired relative heights of a plurality of ground contact portionsbelonging to the C-th node are determined for each C-th node such thatthe relative relationship among the actual node floor reaction forces ofeach node of the C-th node approximates the relative relationship amongthe desired node floor reaction forces of the child nodes of the C-thnode. Then, based on a correction amount obtained by combining thosecorrection amounts of all the C-th nodes, a corrected desired motion isdetermined by adding a first correction of the desired relative heightsof the plurality of ground contact portions belonging to each C-th nodeto the desired motion of the desired gait.

Then, the joints of the mobile body are operated such that thedetermined corrected desired motion is satisfied.

This makes it possible to adjust the relative heights of the groundcontact portions such that the relative relationship among the actualnode floor reaction forces of the child nodes of the C-th nodeapproximates the relative relationship among the desired node floorreaction forces of the child nodes of the C-th node while accomplishingmutual balance of the floor reaction forces of the plurality of groundcontact portions, as compared with a case where the heights ofindividual ground contact portions are independently (separately)manipulated. As a result, even if a floor with which a mobile body is tobe in contact has a locally unknown tilt, projection or depression, aninfluence thereof will be minimized, permitting enhanced stability of aposture of a robot to be achieved.

According to a second invention of a control device for a mobile body ofthe present invention, a control device for a mobile body that comprisesa desired gait determining means for determining a desired gait composedof a desired motion and a desired floor reaction force of a mobile body,such as a mobile robot having three or more ground contact portionsconnected through the intermediary of a plurality of joints such thatmutual relative heights thereof can be manipulated, and a floor reactionforce detecting means for detecting or estimating an actual floorreaction force acting on each of the ground contact portions, andcontrols the operation of the mobile body to make a gait of the mobilebody follow a desired gait,

wherein the ground contact portions are classified into a tree structuresuch that each of the ground contact portions becomes a leaf node, andan intermediate node exists between the leaf node and a root node havingall the leaf nodes as descendant nodes, and

relative to each of B-th nodes, which is each node in the treestructure, if the B-th node is the leaf node, then a floor reactionforce acting on a ground contact portion that is the leaf node isdefined as the node floor reaction force of the B-th node, and if a B-thnode has child nodes, then the resultant force of the node reactionforces of all child nodes of the B-th node is defined as the node floorreaction force of the B-th node,

the control device for a mobile body comprises:

relative to a predetermined C-th node that is an intermediate nodehaving a plurality of ground contact portions as descendant nodes,

a means for determining an actual node floor reaction force as theactual value of a node floor reaction force of each child node of atleast the C-th node from the detected or estimated actual floor reactionforce of each ground contact portion;

a means for determining a desired node floor reaction force as thedesired value of a node floor reaction force of each child node of atleast the C-th node on the basis of at least a desired floor reactionforce of the desired gait; and

a node operation controlling means that determines a corrected desiredmotion by adding at least a first correction of the desired relativeheights of the plurality of ground contact portions belonging to theC-th node to the desired motion of the desired gait on the basis of atleast a relative relationship among the actual node floor reactionforces of the child nodes of the C-th node and a relative relationshipamong the desired node floor reaction forces of the child nodes of theC-th node such that the relative relationship among the actual nodefloor reaction forces of the child nodes of the C-th node approximatesthe relative relationship among the desired node floor reaction forcesof the child nodes of the C-th node, and operates the joints such thatthe corrected desired motion that has been determined is satisfied.

According to the second invention, three or more ground contact portionsprovided in a mobile body are classified into a tree structure, and acorrected desired motion is determined by adding at least a firstcorrection of the desired relative heights of a plurality of groundcontact portions belonging to a C-th node, which is an intermediatenode, to the desired motion of the desired gait on the basis of therelative relationship among the actual node floor reaction forces of thenodes of a predetermined C-th node and the relative relationship amongthe desired node floor reaction forces of the child nodes of the C-thnode (e.g., such that the relative relationships approximate). Then, thejoints of the mobile body are operated so that the determined correcteddesired motion is satisfied. This makes it possible to adjust therelative heights of the ground contact portions such that the relativerelationship among the actual node floor reaction forces of the childnodes of the C-th node approximates the relative relationship among thedesired node floor reaction forces of the child nodes of the C-th nodewhile accomplishing mutual balance of the floor reaction forces of theplurality of ground contact portions belonging to the C-th node, ascompared with a case where the heights of individual ground contactportions are independently (separately) manipulated. As a result, evenif a floor with which a mobile body is to be in contact has a locallyunknown tilt, projection or depression, an influence thereof will beminimized, permitting enhanced stability of a posture of a robot to beachieved.

In the first invention described above, preferably, when a weight hasbeen set on each node in the tree structure, and regarding a B-th node,which is each node in the tree structure, if the B-th node is the leafnode, then the height of a ground contact portion that is the leaf nodeis defined as the height of the B-th node, and if the B-th node haschild nodes, then a weighted mean value, which uses the weight, of theheights of all child nodes of the B-th node is defined as the height ofthe B-th node,

if the node operation controlling means determines the corrected desiredmotion for only one arbitrary node among the C-th nodes by adding afirst correction of the desired relative heights of a plurality ofground contact portions belonging to that one node to a desired motionof the desired gait, then a desired height of that one node in thecorrected desired motion is maintained at a desired height in thedesired motion (a third invention).

Similarly, in the second invention, preferably, a weight has been set oneach node in the tree structure, and relative to the B-th node, which iseach node in the tree structure, if the B-th node is a leaf node, thenthe height of a ground contact portion that is the leaf node is definedas the height of the B-th node, and if the B-th node has child nodes,then the weighted mean value, which uses the above weight, of theheights of all child nodes of the B-th node is defined as the height ofthe B-th node, and

if the node operation controlling means determines, only on the C-thnode, the corrected desired motion by adding a first correction of thedesired relative heights of a plurality of ground contact portionsbelonging to the C-th node to the desired motion, then the desiredheight of the C-th node in the corrected desired motion is maintained atthe desired height in the desired motion (a fourth invention).

According to the third invention, the desired height of each C-th nodetakes the weighted mean value of the desired heights of the child nodesof the C-th node (the desired heights are determined on the basis of thedesired heights of the ground contact portions belonging to the C-thnode in the aforesaid desired motion). If the corrected desired motionis determined for only one arbitrary node among the C-th nodes by addinga first correction of the desired relative heights of the plurality ofground contact portions belonging to that one node to the desired motionof the aforesaid desired gait, then the first correction is added to adesired motion such that the aforesaid weighted mean value of the childnodes of the aforesaid one node will be the same in a corrected desiredmotion and a desired motion.

Similarly, according to the fourth invention, if the corrected desiredmotion is determined, only on the C-th node, by adding a firstcorrection of the desired relative heights of a plurality of groundcontact portions belonging to the C-th node to the desired motion, thenthe first correction is added to a desired motion such that theaforesaid weighted mean value of the child nodes of the C-th node willbe the same in a corrected desired motion and in a desired motion.

As a result, the desired relative heights of the plurality of groundcontact portions belonging to a C-th node can be corrected whileachieving mutual balance among the floor reaction forces of theplurality of ground contact portions belonging to the C-th node. Thiseventually permits further improved stability of a posture of the robot.

According to a fifth invention of the control device for a mobile bodyof the present invention, a control device for a mobile body thatcomprises a desired gait determining means for determining a desiredgait composed of a desired motion and a desired floor reaction force ofa mobile body, such as a mobile robot having three or more groundcontact portions connected through the intermediary of a plurality ofjoints such that mutual relative heights thereof can be manipulated, anda floor reaction force detecting means for detecting or estimating anactual floor reaction force acting on each of the ground contactportions, and controls the operation of the mobile body to make a gaitof the mobile body follow a desired gait,

wherein the ground contact portions are classified into a tree structuresuch that each of the ground contact portions becomes a leaf node, andan intermediate node exists between the leaf node and a root node havingall the leaf nodes as descendant nodes, and

relative to a B-th node, which is each node in the tree structure, ifthe B-th node is the leaf node, then a floor reaction force acting on aground contact portion that is the leaf node is defined as the nodefloor reaction force of the B-th node, and if the B-th node has childnodes, then the resultant force of the node floor reaction forces of allchild nodes of the B-th node is defined as the node floor reaction forceof the B-th node, and further, if the point at which the horizontalcomponent or a parallel-to-floor component of the moment of the nodefloor reaction force of the B-th node becomes zero is defined as thenode floor reaction force central point of the B-th node,

the control device for a mobile body, comprising:

a means for determining a desired node floor reaction force centralpoint, which is the desired position of a node floor reaction forcecentral point of each node, on the basis of at least one of the desiredmotion and the desired floor reaction force of the desired gait;

a means for determining an actual node floor reaction force as theactual value of a node floor reaction force of each node from the actualfloor reaction force of each ground contact portion that has beendetected or estimated; and

a node operation controlling means that determines,

on each of the C-th nodes, which is a node having a plurality of groundcontact portions as descendant nodes,

by using, as a control amount, one of

a difference (Mc_act−Mc_dmd) between an actual moment (Mc_act), which isthe moment caused by the actual node floor reaction force of the C-thnode to act on a desired node floor reaction force central point of theC-th node, and a desired value (Mc_dmd) of a moment that should act onthe desired node floor reaction force central point of the C-th node,

a difference (Mc_f−Mc_dmd) between a moment (Mc_f), which is obtained bysubtracting the moment caused by the actual node floor reaction force ofeach child node of the C-th node to act on a desired node floor reactionforce central point of the child node from the actual moment (Mc_act) ofthe C-th node, and a desired value (Mc_dmd) of a moment that should acton the desired node floor reaction force central point of the C-th node,a value of the difference (ΔP) between the position of the point atwhich the horizontal component or the parallel-to-floor component of themoment of the actual node floor reaction force of the C-th node becomeszero and the position of the desired node floor reaction force centralpoint of the C-th node, and

a value of the difference (ΔPf) between the position of the point atwhich the horizontal component or the parallel-to-floor component of themoment, which is obtained by subtracting the moment caused by the actualnode floor reaction force of each child node of the C-th node to act onthe desired node floor reaction force central point of the child nodefrom the moment of the actual node floor reaction force (Mc_act) of theC-th node, becomes zero and the position of the desired node floorreaction force central point of the C-th node,

a correction amount of the desired relative heights of a plurality ofground contact portions belonging to the C-th node, for each C-th node,such that the control amount approximates zero on the basis of thecontrol amount, and also determines, on the basis of a correction amountobtained by combining the determined correction amount of every C-thnode, a corrected desired motion obtained by adding at least a firstcorrection of the desired relative heights of the plurality of groundcontact portions belonging to each C-th node to a desired motion of thedesired gait, then operates the joints such that the determinedcorrected desired motion is satisfied.

According to the fifth invention, three or more ground contact portionsprovided in a mobile body are classified into a tree structure, and thecorrection amounts (in other words, manipulated variables) of thedesired relative heights of a plurality of ground contact portionsbelonging to the C-th node are determined for each C-th node such thatthe moment acting on a desired floor reaction force central point ofeach C-th node having child nodes or the aforesaid control amountrelated to the position of the desired floor reaction force centralpoint approximates zero. Then, based on a correction amount obtained bycombining those correction amounts of all the C-th nodes, a correcteddesired motion is determined by adding at least a first correction ofthe desired relative heights of the plurality of ground contact portionsbelonging to each C-th node to the desired motion of the desired gait,and the joints of the mobile body are operated such that the determinedcorrected desired motion is satisfied. This makes it possible to adjustthe relative heights of the ground contact portions such that therelative relationship among the actual node floor reaction forces of thechild nodes of the C-th node approximates the relative relationshipamong the desired node floor reaction forces of the child nodes of theC-th node while accomplishing mutual balance of the floor reactionforces of the plurality of ground contact portions, as compared with acase where the heights of individual ground contact portions areindependently (separately) manipulated. As a result, even if a floorwith which a mobile body is to be in contact has a local unknown tilt,projection or depression, an influence thereof will be minimized,permitting enhanced stability of a posture of a robot to be achieved.

According to a sixth invention of the control device for a mobile bodyof the present invention, a control device for a mobile body thatcomprises a desired gait determining means for determining a desiredgait composed of a desired motion and a desired floor reaction force ofa mobile body, such as a mobile robot having three or more groundcontact portions connected through the intermediary of a plurality ofjoints such that mutual relative heights thereof can be manipulated, anda floor reaction force detecting means for detecting or estimating anactual floor reaction force acting on each of the ground contactportions, and controls the operation of the mobile body to make a gaitof the mobile body follow a desired gait,

wherein the ground contact portions are classified into a tree structuresuch that each of the ground contact portions becomes a leaf node, andan intermediate node exists between the leaf node and a root node havingall the leaf nodes as descendant nodes, and

relative to each of B-th nodes, which is each node in the treestructure, if the B-th node is the leaf node, then a floor reactionforce acting on a ground contact portion that is the leaf node isdefined as the node floor reaction force of the B-th node, and if theB-th node has child nodes, then the resultant force of the node floorreaction forces of all child nodes of the B-th node is defined as thenode floor reaction force of the B-th node, and further, if the point atwhich the horizontal component or a parallel-to-floor component of themoment of the node floor reaction force of the B-th node becomes zero isdefined as the node floor reaction force central point of the B-th node,

the control device for a mobile body, comprising:

relative to a predetermined C-th node that is an intermediate nodehaving a plurality of ground contact portions as descendant nodes,

a means for determining a desired node floor reaction force centralpoint, which is the desired position of a node floor reaction forcecentral point of at least the C-th node, on the basis of at least thedesired gait;

a means for determining an actual node floor reaction force as theactual value of a node floor reaction force of at least the C-th nodefrom the detected or estimated actual floor reaction force of eachground contact portion; and

a node operation controlling means that determines,

by using, as a control amount, one of

a difference (Mc_act−Mc_dmd) between an actual moment (Mc_act), which isthe moment caused by the actual node floor reaction force of the C-thnode to act on a desired node floor reaction force central point of theC-th node, and a desired value (Mc_dmd) of a moment that should act onthe desired node floor reaction force central point of the C-th node,

a difference (Mc_f−Mc_dmd) between a moment (Mc_f), which is obtained bysubtracting the moment caused by the actual node floor reaction force ofeach child node of the C-th node to act on a desired node floor reactionforce central point of the child node from the actual moment (Mc_act) ofthe C-th node, and a desired value (Mc_dmd) of a moment that should acton the desired node floor reaction force central point of the C-th node,

a value of the difference (ΔP) between the position of the point atwhich the horizontal component or the parallel-to-floor component of themoment of the actual node floor reaction force of the C-th node becomeszero and the position of the desired node floor reaction force centralpoint of the C-th node, and

a value of the difference (ΔPf) between the position of the point atwhich the horizontal component or the parallel-to-floor component of themoment, which is obtained by subtracting the moment caused by the actualnode floor reaction force of each child node of the C-th node to act onthe desired node floor reaction force central point of the child nodefrom the moment of the actual node floor reaction force (Mc_act) of theC-th node, becomes zero and the position of the desired node floorreaction force central point of the C-th node,

a corrected desired motion by adding at least a first correction of thedesired relative heights of a plurality of ground contact portionsbelonging to the C-th node to the desired motion of the desired gaitsuch that the control amount approximates zero on the basis of thecontrol amount, and operates the joints such that the determinedcorrected desired motion is satisfied.

According to the sixth invention, three or more ground contact portionsprovided in a mobile body are classified into a tree structure, and acorrected desired motion is determined by adding at least a firstcorrection of the desired relative heights of a plurality of groundcontact portions belonging to the C-th node, which is an intermediatenode, to the desired motion of the desired gait such that the momentacting on a desired floor reaction force central point of a certainpredetermined C-th node or the aforesaid control amount related to theposition of a desired floor reaction force central point approximateszero. Then, the joints of the mobile body are operated such that thedetermined corrected desired motion is satisfied. This makes it possibleto adjust the relative heights of the ground contact portions such thatthe relative relationship among the actual node floor reaction forces ofthe child nodes of the C-th node approximates the relative relationshipamong the desired node floor reaction forces of the child nodes of theC-th node while accomplishing mutual balance of the floor reactionforces of the plurality of ground contact portions, as compared with acase where the heights of individual ground contact portions areindependently (separately) manipulated. As a result, even if a floorwith which a mobile body is to be in contact has a local unknown tilt,projection or depression, an influence thereof will be minimized,permitting enhanced stability of a posture of a robot to be achieved.The joints of the mobile body are operated such that the correcteddesired relative heights are satisfied. This makes it possible tomanipulate the relative heights of the ground contact portions such thatthe moment acting on the desired floor reaction force central point ofthe C-th node or the aforesaid control amount related to the position ofa desired floor reaction force central point approximates zero whileaccomplishing mutual balance of the floor reaction forces of theplurality of ground contact portions, as compared with a case where theheights of individual ground contact portions are independently(separately) manipulated. As a result, even if a floor with which amobile body is to be in contact has a local unknown tilt, projection ordepression, an influence thereof will be minimized, permitting enhancedstability of a posture of a robot to be achieved.

In the aforesaid fifth invention, preferably, a weight has been set oneach node in the tree structure, and regarding a B-th node, which iseach node in the tree structure, if the B-th node is the leaf node, thenthe height of a ground contact portion that is the leaf node is definedas the height of the B-th node, and if the B-th node has child nodes,then a weighted mean value, which uses the weight, of the heights of allchild nodes of the B-th node is defined as the height of the B-th node,and

when the node operation controlling means determines the correcteddesired motion for only one arbitrary node among C-th nodes by adding afirst correction of the desired relative heights of the plurality ofground contact portions belonging to that one node to the desiredmotion, then the desired height of that one node in the correcteddesired motion is maintained at a desired height in the desired motion(a seventh invention).

Similarly, in the sixth invention, preferably, a weight has been set oneach node in the tree structure, and regarding a B-th node, which iseach node in the tree structure, if the B-th node is the leaf node, thenthe height of the ground contact portion that is the leaf node isdefined as the height of the B-th node, and if the B-th node has childnodes, then the weighted mean value, which uses the above weight, of theheights of all child nodes of the B-th node is defined as the height ofthe B-th node, and

when the node operation controlling means determines, only on the C-thnode, the corrected desired motion by adding a first correction of thedesired relative heights of a plurality of ground contact portionsbelonging to the C-th node to the desired motion, the desired height ofthe C-th node in the corrected desired motion is maintained at thedesired height in the desired motion (an eighth invention).

According to the seventh invention, the desired height of each C-th nodetakes the weighted mean value of the desired heights of the child nodesof the C-th node (the desired heights are determined on the basis of thedesired heights of the ground contact portions belonging to the C-thnode in the aforesaid desired motion). If the corrected desired motionis determined for only one arbitrary node among the C-th nodes by addinga first correction of the desired relative heights of the plurality ofground contact portions belonging to that one node to the desired motionof the aforesaid desired gait, then the first correction is added to adesired motion such that the aforesaid weighted mean value of the childnodes of the aforesaid one node will be the same in a corrected desiredmotion and in a desired motion.

Similarly, according to the eighth invention, if the corrected desiredmotion is determined only on the C-th node by adding a first correctionof the desired relative heights of a plurality of ground contactportions belonging to the C-th node to the desired motion, then thefirst correction is added to a desired motion such that the aforesaidweighted mean value of the child nodes of the C-th node will be the samein a corrected desired motion and in a desired motion.

As a result, the desired relative heights of the plurality of groundcontact portions belonging to a C-th node can be corrected whileachieving mutual balance among the floor reaction forces of theplurality of ground contact portions belonging to the C-th node. Thiseventually permits further improved stability of a posture of the robot.

In the aforesaid seventh and eighth inventions, the means fordetermining a desired node floor reaction force central point preferablydetermines the desired node floor reaction force central points andweights of the C-th node and each of the descendant nodes thereof suchthat the weighted mean value, which uses the aforesaid weight, of thepositions of the desired node floor reaction force central points of thechild nodes of the C-th node will be the desired node floor reactionforce central point of the C-th node (a ninth invention).

In the aforesaid fifth invention, the node operation controlling meanspreferably determines, for each of the C-th nodes, the correctionamounts of the desired relative heights of the plurality of groundcontact portions belonging to the C-th node on the basis of a movementamount of the desired node floor reaction force central point of eachchild node of the C-th node, the movement amount being obtained when thedesired node floor reaction force central point of each child node ofthe C-th node is rotated about the desired node floor reaction forcecentral point of the C-th node by the rotational amount determined onthe basis of at least the aforesaid control amount (a tenth invention).

Similarly, in the aforesaid sixth invention, the node operationcontrolling means preferably determines the aforesaid corrected desiredmotion obtained by adding a first correction of the desired relativeheights of the plurality of ground contact portions belonging to theC-th node to the desired motion on the basis of the movement amount ofthe desired node floor reaction force central point of each child nodeof the C-th node, the movement amount being obtained when the desirednode floor reaction force central point of each child node of the C-thnode is rotated about the desired node floor reaction force centralpoint of the C-th node by the rotational amount determined on the basisof at least the aforesaid control amount (an eleventh invention).

Similarly, in the aforesaid seventh invention, the node operationcontrolling means preferably determines a corrected desired motion,which is obtained by adding a first correction of the desired relativeheights of the plurality of ground contact portions belonging to theC-th node to the desired motion, by manipulating the mutual relativeheights of the child nodes of the C-th node on the basis of a movementamount of the desired node floor reaction force central point of eachchild node of the C-th node, the movement amount being obtained when thedesired node floor reaction force central point of each child node ofthe C-th node is rotated about the desired node floor reaction forcecentral point of the C-th node by a rotational amount determined on thebasis of at least the aforesaid control amount (a twelfth invention).

These tenth to twelfth inventions make it possible to easily determine afirst correction of the relative heights of the plurality of groundcontact portions belonging to the C-th node such that the desired heightof the C-th node will be the same in a corrected desired motion and in adesired motion (the desired height of the C-th node in the correcteddesired motion will be maintained at a desired height in a desiredmotion).

In the third, the fourth, the seventh, and the eighth inventionsdescribed above, relative to each node having child nodes in theaforesaid tree structure, the weights of all child nodes of the eachnode are preferably set such that the total sum of the weights of allthe child nodes becomes one (a thirteenth invention).

Further, preferably, the third, the fourth, the seventh, and the eighthinventions described above comprise a means for variably setting theweight of the aforesaid each node, wherein if a leaf node correspondingto each ground contact portion not in contact with ground is defined asa D-th node, then the aforesaid means for setting weights sets theweight of each D-th node or the weight of at least one node of theancestor nodes of the D-th node to zero (a fourteenth invention).

The fourteenth invention makes it possible to prevent wastefulcorrections of the heights of ground contact portions not in contactwith the ground, which cannot contribute to bringing the relativerelationship among the actual node floor reaction forces of the childnodes of a C-th node close to the relative relationship among thedesired node floor reaction forces of the child nodes of the C-th nodeor bringing the aforesaid control amount to zero. Incidentally, thefourteenth invention may be combined with the thirteenth invention.

Further, preferably, the first, the second, the fifth, and the sixthinventions described above (or the first to the fourteenth inventions)comprise an actual posture inclination detecting means for detecting orestimating the actual posture inclination of a predetermined portion,such as the base body, of the aforesaid mobile body,

wherein, when determining the aforesaid corrected desired motion, thenode operation controlling means determines the aforesaid correcteddesired motion by further adding, to the aforesaid desired motion, acorrection of the desired relative heights of the plurality of groundcontact portions belonging to the aforesaid C-th node on the basis of adifference between the aforesaid actual posture inclination and thedesired posture inclination of the aforesaid predetermined portion inthe desired motion of the aforesaid desired gait such that thedifference approximates zero (a fifteenth invention).

According to the fifteenth invention, the aforesaid corrected desiredmotion is determined by further adding a correction of the desiredrelative heights of a plurality of ground contact portions belonging toa C-th node to a desired motion such that the aforesaid postureinclination error approximates zero; therefore, the stability of theposture of a robot can be further enhanced.

Further, especially the fifth invention and the sixth inventionsdescribed above may comprise an actual posture inclination detectingmeans for detecting or estimating the actual posture inclination of apredetermined portion, such as the base body, of the aforesaid mobilebody,

wherein the aforesaid node operation controlling means may comprise ameans for correcting at least either the desired value of the momentthat should act on the aforesaid desired node floor reaction forcecentral point of the aforesaid C-th node or the desired node floorreaction force central point of the C-th node on the basis of a postureinclination error, which is the difference between the aforesaid actualposture inclination and the desired posture inclination of the aforesaidpredetermined portion in the aforesaid desired motion, such that theposture inclination error approximates zero, and it may determine theaforesaid control amount by using the aforesaid desired value of amoment or the aforesaid desired node floor reaction force central point,which has been corrected, in place of the desired value of a moment or adesired node floor reaction force central point before the correction (asixteenth invention).

According to the sixteenth invention, the desired value of a moment thatshould act on the desired floor reaction force central point of a C-thnode or the position of the desired floor reaction force central pointis corrected such that the aforesaid posture inclination errorapproximates zero, and then the corrected moment desired value ordesired node floor reaction force central point is used in place of themoment desired value or desired node floor reaction force central pointbefore the correction to determine the aforesaid control amount;therefore, a corrected desired motion is determined by adding a firstcorrection related to the aforesaid C-th node for bringing the controlamount close to zero to a desired motion. As a result, the stability ofthe posture of a mobile body can be enhanced while properly controllinga floor reaction force acting on the mobile body such that the aforesaidposture inclination error approximates zero. Incidentally, the sixteenthinvention may be combined with the seventh to the fourteenth inventionsdescribed above.

Further, the fifth invention and the sixth invention preferably have theaforesaid ground contact portions connected to a base body of theaforesaid mobile body through the intermediary of connection mechanismssuch that they are movable with respect to the base body, and comprise:

a desired node floor reaction force distributing means for determining,on each E-th node, which is a node having child nodes in the aforesaidtree structure, the translational force component of a desired nodefloor reaction force of each leaf node belonging to the E-th node on thebasis of at least the desired floor reaction force of the aforesaiddesired gait such that the horizontal component or the parallel-to-floorcomponent of a moment caused by a desired node floor reaction force,which is the desired value of the E-th node floor reaction force, to acton the aforesaid desired node floor reaction force central point of theE-th node becomes zero,

wherein the aforesaid node operation controlling means comprises a meansfor estimating, on the basis of at least the translational forcecomponent of a desired node floor reaction force of each leaf nodebelonging to the aforesaid C-th node, the amount of a deformation thatoccurs in the connecting mechanism between the aforesaid base body andeach ground contact portion belonging to the C-th node and in the groundcontact portion when the translational force component of the desirednode floor reaction force acts on the ground contact portion belongingto the C-th node, and when determining the aforesaid corrected desiredmotion, it determines the aforesaid corrected desired motion by furtheradding, to the aforesaid desired motion, a second correction of thedesired height of each of a plurality of ground contact portionsbelonging to the aforesaid C-th node to cancel the aforesaid estimateddeformation amount (a seventeenth invention).

The seventeenth invention considers deformations, such as flexure,taking place in the aforesaid connecting mechanisms or ground contactportions due to floor reaction forces applied when the ground contactportions come in contact with the ground, and estimates the deformationamounts on the basis of the translational force components of thedesired node floor reaction forces of leaf nodes (ground contactportions). Then, a second correction of the desired height of each ofthe plurality of ground contact portions belonging to the aforesaid C-thnode is further added to the aforesaid desired motion to cancel theestimated deformation amount, thereby determining a corrected desiredmotion. Thus, according to the seventeenth invention, proper floorreaction forces can be applied to a mobile body while compensating forthe influences of deformations of connecting mechanisms or groundcontact portions, thus making it possible to further enhance thestability of a posture of the mobile body.

Preferably, the third, the fourth, the seventh, and the eighthinventions described above have the aforesaid ground contact portionsconnected to the base body of the aforesaid mobile body through theintermediary of connecting mechanisms such that they are movable withrespect to the base body, and comprise:

a desired node floor reaction force distributing means that determines,on each of E-th nodes, which is a node having a parent node in theaforesaid tree structure, the translational force component of a desirednode floor reaction force of the E-th node on the basis of at least thedesired floor reaction force of the aforesaid desired gait such that thetranslational force component of a desired node floor reaction force,which is the desired value of the E-th node floor reaction force, takesa value obtained by multiplying the translational force component of adesired node floor reaction force of the parent node of the E-th node bythe aforesaid weight of the E-th node,

wherein the aforesaid node operation controlling means comprises a meansfor estimating, on the basis of at least the translational forcecomponent of a desired node floor reaction force of each leaf nodebelonging to the aforesaid C-th node, the amounts of deformations thatoccur in the connecting mechanisms between the aforesaid base body andthe ground contact portions belonging to the C-th node and in the groundcontact portions when the translational force component of the desirednode floor reaction force acts on the ground contact portions belongingto the C-th node, and when determining the aforesaid corrected desiredmotion, it determines the aforesaid corrected desired motion by furtheradding, to the aforesaid desired motion, a second correction of thedesired height of each of a plurality of ground contact portionsbelonging to the aforesaid C-th node to cancel the aforesaid estimateddeformation amounts (an eighteenth invention).

As with the seventeenth invention, the eighteenth invention considersdeformations, such as flexure, taking place in the aforesaid connectingmechanisms or ground contact portions due to floor reaction forcesapplied when the ground contact portions come in contact with theground, and estimates the deformation amounts on the basis of thetranslational force components of the desired node floor reaction forcesof leaf nodes (ground contact portions). Then, a second correction ofthe desired height of each of the plurality of ground contact portionsbelonging to the aforesaid C-th node is further added to the aforesaiddesired motion to cancel the estimated deformation amount, therebydetermining the aforesaid corrected desired motion. Hence, according tothe eighteenth invention, proper floor reaction forces can be applied toa mobile body while compensating for the influences of deformations ofconnecting mechanisms or ground contact portions, thus making itpossible to further enhance the stability of a posture of the mobilebody.

Supplementally, the desired node floor reaction force distributing meansin the aforesaid seventeenth invention and the desired node floorreaction force distributing means in the aforesaid eighteenth inventionwill eventually determine the desired node floor reaction force of aleaf node in the same manner.

Further, preferably, the fifth and the sixth inventions described abovehave the ground contact portions connected to the base body of themobile body through the intermediary of connecting mechanisms such thatthey are movable with respect to the base body, and further comprise:

an actual posture inclination detecting means for detecting orestimating the actual posture inclination of a predetermined portion,such as the base body, of the aforesaid mobile body;

a means for correcting at least the desired node floor reaction forcecentral point of the aforesaid C-th node on the basis of a postureinclination error, which is the difference between the actual postureinclination and the desired posture inclination of the aforesaidpredetermined portion in the aforesaid desired motion, such that theposture inclination error approximates zero; and

a desired node floor reaction force distributing means that determinesat least the translational force component of a desired node floorreaction force, which is the desired value of the node floor reactionforce of each descendant node of the aforesaid C-th node, out of thedesired node floor reaction force, which is the desired value of thenode floor reaction force of each node, on the basis of at least thedesired floor reaction force of the aforesaid desired gait such that thehorizontal component or the parallel-to-floor component of the moment ofa desired node floor reaction force acting on the aforesaid correcteddesired node floor reaction force central point of the C-th node becomeszero,

wherein the aforesaid node operation controlling means comprises a meansfor estimating, on the basis of at least the translational forcecomponent of a desired node floor reaction force of each leaf nodebelonging to the aforesaid C-th node, the amounts of deformations thatoccur in the connecting mechanisms between the aforesaid base body andthe ground contact portions belonging to the C-th node and in the groundcontact portions when the translational force component of the desirednode floor reaction force acts on the ground contact portions belongingto the C-th node, and when determining the aforesaid corrected desiredmotion, it determines the aforesaid control amount by using theaforesaid corrected desired node floor reaction force central point ofthe C-th node in place of the desired node floor reaction force centralpoint before the correction and also determines the aforesaid correcteddesired motion by further adding, to the aforesaid desired motion, asecond correction of the desired height of each of a plurality of groundcontact portions belonging to the aforesaid C-th node to cancel theaforesaid estimated deformation amounts (a nineteenth invention).

Alternatively, when the aforesaid ground contact portions are connectedto the base body of the aforesaid mobile body through the intermediaryof connecting mechanisms such that they are movable with respect to thebase body, preferably, the fifth and the sixth inventions describedabove comprise:

an actual posture inclination detecting means for detecting orestimating the actual posture inclination of a predetermined portion,such as the base body, of the aforesaid mobile body;

a means for determining the desired value of a moment that should act onthe desired floor reaction force central point of at least the aforesaidC-th node on the basis of a posture inclination error, which is thedifference between the aforesaid actual posture inclination and thedesired posture inclination of the aforesaid predetermined portion inthe aforesaid desired motion, such that the posture inclination errorapproximates zero; and

a desired node floor reaction force distributing means that determinesat least the translational force component of a desired node floorreaction force, which is the desired value of a node floor reactionforce of each descendant node of the aforesaid C-th node, out of thedesired node floor reaction force, which is the desired value of a nodefloor reaction force of each node, on the basis of at least a floorreaction force of the aforesaid desired gait such that the moment of adesired node floor reaction force acting on the aforesaid desired nodefloor reaction force central point of the C-th node becomes theaforesaid determined desired value,

wherein the aforesaid node operation controlling means comprises a meansfor estimating, on the basis of at least the translational forcecomponent of a desired node floor reaction force of each leaf nodebelonging to the aforesaid C-th node, the amount of a deformation thatoccurs in a connecting mechanism between the aforesaid base body andeach ground contact portion belonging to the C-th node and in the groundcontact portion when the translational force component of the desirednode floor reaction force acts on each ground contact portion belongingto the C-th node, and when determining the aforesaid corrected desiredmotion, it determines the aforesaid control amount by using theaforesaid determined desired value of the moment of the C-th node andalso determines the aforesaid corrected desired motion by furtheradding, to the aforesaid desired motion, a second correction of thedesired height of each of a plurality of ground contact portionsbelonging to the aforesaid C-th node to cancel the aforesaid estimateddeformation amount (a twentieth invention).

According to the nineteenth invention and the twentieth invention, aswith the aforesaid sixteenth invention, the first correction of thedesired relative heights of the ground contact portions belonging to theC-th node is added to a desired motion such that the aforesaid controlamount that contains a correction for bringing the aforesaid postureinclination error close to zero approximates zero. At the same time, aswith the aforesaid seventeenth invention, the second correction of thedesired heights of the ground contact portions belonging to the C-thnode is further added to the desired motion so as to determine acorrected desired motion. Thus, according to the nineteenth and thetwentieth inventions, proper floor reaction forces can be applied to amobile body while compensating for the influences of deformations ofconnecting mechanisms or ground contact portions and also making theaforesaid posture inclination error approximate zero, thus making itpossible to further enhance the stability of a posture of the mobilebody.

Further, when the aforesaid ground contact portions are connected to thebase body of the aforesaid mobile body through the intermediary ofconnecting mechanisms such that they are movable with respect to thebase body, the third, the fourth, the seventh, and the eighth inventionsdescribed above, in which a weight has been set on each node, preferablycomprise:

an actual posture inclination detecting means for detecting orestimating the actual posture inclination of a predetermined portion,such as the base body, of the aforesaid mobile body;

a means for correcting at least the weight of each descendant node ofthe aforesaid C-th node on the basis of the posture inclination error,which is the difference between the aforesaid actual posture inclinationand the desired posture of the aforesaid predetermined portion in theaforesaid desired motion, such that the posture inclination errorapproximates zero; and

a desired node floor reaction force distributing means that determinesat least the translational force component of a desired node floorreaction force, which is the desired value of a node floor reactionforce of each descendant node of the aforesaid C-th node, out of thedesired node floor reaction force, which is the desired value of thenode floor reaction force of each node, on the basis of at least adesired floor reaction force of the aforesaid desired gait such that ittakes a value obtained by multiplying the translational force componentof a desired node floor reaction force of a parent node of eachdescendant node by the aforesaid corrected weight of the descendantnode,

wherein the node operation controlling means comprises a means forestimating, on the basis of at least the translational force componentof a desired node floor reaction force of each leaf node belonging tothe aforesaid C-th node, the amount of a deformation that occurs in aconnecting mechanism between the aforesaid base body and each groundcontact portion belonging to the C-th node and in the ground contactportion when the translational force component of the desired node floorreaction force acts on each ground contact portion belonging to the C-thnode, and when determining the aforesaid corrected desired motion, itdetermines the aforesaid corrected desired motion by further adding, tothe aforesaid desired motion, a second correction of the desired heightof each of a plurality of ground contact portions belonging to the C-thnode to cancel the estimated deformation amount (an twenty-firstinvention).

According to this twenty-first invention, the weight of each descendantnode of the C-th node is corrected such that a posture inclination errorapproximates zero; therefore, the weight is used to determine acorrected desired motion by adding the first correction of the desiredrelative heights of the ground contact portions belonging to the C-thnode to a desired motion, as in the third, the fourth, the seventh, andthe eighth inventions described above, thereby determining a correcteddesired motion such that a proper floor reaction force acts on a mobilebody while bringing a posture inclination error close to zero. At thesame time, as with the aforesaid eighteenth invention, the secondcorrection of the desired heights of the ground contact portionsbelonging to the C-th node is further added to the desired motion todetermine a corrected desired motion. Thus, the corrected desired motionis the desired motion that has been corrected to cancel the deformationamounts of the aforesaid connecting mechanisms or ground contactportions. Hence, according to the twenty-first invention, as with thenineteenth and the twentieth inventions, proper floor reaction forcescan be applied to a mobile body while compensating for the influences ofdeformations of connecting mechanisms or ground contact portions andalso making the aforesaid posture inclination error approximate zero,thus making it possible to further enhance the stability of a posture ofthe mobile body.

Supplementally, the desired node floor reaction force distributing meansin the aforesaid twenty-first invention, the desired node floor reactionforce distributing means in the aforesaid nineteenth invention, and thedesired node floor reaction force distributing means in the aforesaidtwentieth invention will eventually determine the desired node floorreaction force of a leaf node in the same manner.

Further, in the first invention and the fifth inventions describedabove, if the aforesaid ground contact portions are connected to thebase body of the aforesaid mobile body such that they are movable withrespect to the base body, then, when determining the aforesaid correcteddesired motion, the aforesaid node operation controlling meanspreferably determines the corrected desired motion by further adding, tothe aforesaid desired motion, a correction of a desired posture of theaforesaid base body to restrain slippages, such as twists, of the groundcontact portions belonging to the aforesaid C-th node on a floor surface(a twenty-second invention). This applies also to the second inventionand the sixth invention described above (a twenty-third invention).

If many ground contact portions are connected to the base body of amobile body, insufficient degrees of joints of the mobile body result,mechanically restricting the corrections of the relative heights of theground contact portions. Hence, if the joints are forcibly operated tosatisfy determined desired relative heights, then slippages, such astwists, may occur at some ground contact portions. And, in such asituation wherein slippages occur, there is a danger in thatinappropriate floor reaction forces of the ground contact portions willresult and the stability of a posture of a robot will be impaired. Insuch a case, as with the twenty-second invention and the twenty-thirdinvention, the aforesaid slippages, such as twists, can be restrained byfurther adding a correction of a desired posture of the base body to adesired motion in determining the aforesaid corrected desired motion.Thus, the stability of the posture of the mobile body can be securedwhile adjusting those floor reaction forces to appropriate ones so thatno undue floor reaction forces act on ground contact portions.

In the aforesaid twenty-second invention, further preferably, theaforesaid node operation controlling means determines the aforesaidcorrected desired motion such that the direction of the segment thatconnects the desired overall floor reaction force central point of theaforesaid mobile body, which depends on a desired floor reaction forceof the aforesaid desired gait, and the overall center-of-gravity of theaforesaid mobile body or a predetermined representative point of thebase body of the mobile body in the aforesaid corrected desired motionis substantially identical to the direction of the segment in theaforesaid desired gait (a twenty-fourth invention). And, this appliesalso to the aforesaid twenty-third invention (a twenty-fifth invention).

According to these twenty-fourth and twenty-fifth inventions, it ispossible to prevent the direction of the resultant force of thegravitational force and an inertial force acting on a mobile body fromconsiderably deviating from an appropriate direction in a desired gaitwhen a desired motion is corrected by further adding a correction of adesired posture of the base body, so that the aforesaid correcteddesired motion can be determined while properly maintaining thestability of the mobile body.

Alternatively, in the aforesaid twenty-second invention, the aforesaidnode operation controlling means may determine the aforesaid correcteddesired motion such that the horizontal position of the overallcenter-of-gravity of the aforesaid mobile body or the horizontalposition of a predetermined representative point of the base body of themobile body in the aforesaid corrected desired motion is substantiallyidentical to the horizontal position in the aforesaid desired motion (atwenty-sixth invention). And, this applies also to the aforesaidtwenty-third invention (a twenty-seventh invention).

According to these twenty-sixth and twenty-seventh inventions, it ispossible to prevent the overall center-of-gravity of a mobile body fromconsiderably deviating in the horizontal direction relative to anappropriate position in a desired gait when a corrected desired motionis determined by adding a second correction of a desired posture of thebase body to a desired motion, so that the relative heights of theground contact portions of a C-th node and the posture of the base bodycan be corrected while properly maintaining the stability of the mobilebody.

Further, the twenty-second invention is suited to a case where theaforesaid mobile body is a robot equipped with a plurality of linkmechanisms extended from its base body, and at least one link mechanismamong the plurality of link mechanisms is provided with a joint at leastin an intermediate portion thereof between its distal end and its endadjacent to the base body, and the intermediate portion is the aforesaidground contact portion (a twenty-eighth invention).

Similarly, the twenty-third invention is suited to a case where theaforesaid mobile body is a robot equipped with a plurality of linkmechanisms extended from its base body, and at least one link mechanismamong the plurality of link mechanisms is provided with a joint at leastin an intermediate portion between its distal end and its end adjacentto the base body, and the intermediate portion is a ground contactportion belonging to the aforesaid C-th node (a twenty-ninth invention).

More specifically, normally, a robot having a plurality of linkmechanisms extended from its base body usually comes in contact with theground at the distal end portions of the link mechanisms; therefore, ifan attempt were made to make it come in contact with the ground at anintermediate portion of the link mechanism (e.g., if an attempt weremade to make human-like mobile robot come in contact with the ground atan intermediate portion of a leg (knee) thereof), then the joint woulddevelop an insufficient degree of freedom, possibly causing a slippage,such as a twist, as mentioned above. Thus, the twenty-second inventionand the twenty-third invention are suited to cases where a mobile bodyis constructed as described in the twenty-eighth invention and thetwenty-ninth invention, respectively.

In the aforesaid twenty-eighth invention, the aforesaid intermediateportion is preferably provided with an elastic member that resilientlydeforms when coming into contact with the ground (a thirtiethinvention). And, this applies also to the aforesaid twenty-ninthinvention (a thirty-first invention).

This enhances the capability of adjusting floor reaction forces bycorrecting the relative heights of the aforesaid intermediate portions,making it easier to secure the stability of the posture of a mobile body(robot).

Further, the aforesaid twenty-second invention is suited to a case wherethe aforesaid mobile body is provided with a plurality of linkmechanisms extended from its base body, each of which having one or morejoints, at least one link mechanism among the aforesaid link mechanismsand the aforesaid base body are provided with ground contact portions,and ground contact portions that belong to at least one of the aforesaidC-th nodes include at least a ground contact portion of the aforesaidbase body and ground contact portions of one or more link mechanismsamong the aforesaid plurality of link mechanisms (a thirty-secondinvention).

Similarly, the aforesaid twenty-third invention is suited to a casewhere the aforesaid mobile body is provided with a plurality of linkmechanisms extended from the base body, each of which having one or morejoints, at least one link mechanism among the aforesaid link mechanismsand the aforesaid base body are provided with ground contact portions,and the ground contact portions that belong to the aforesaid C-th nodeinclude at least a ground contact portion of the aforesaid base body andground contact portions of one or more link mechanisms among theaforesaid plurality of link mechanisms (a thirty-third invention).

For example, if a robot having one or more legs (link mechanisms)extended from its base body (upper body or the like) is to perform asitting operation, it will have its base body to come in contact. Insuch a case, the degree of freedom of a joint will be insufficient, andits base body will come in contact, and if this happens, the aforesaidslippage, such as a twist, may take place.

Accordingly, the twenty-second invention and the twenty-third inventionare suited also to cases where mobile bodies are constructed asdescribed in the thirty-second invention and the thirty-third invention,respectively.

In the aforesaid thirty-second invention, a ground contact portion ofthe aforesaid base body is preferably provided with an elastic memberthat resiliently deforms when coming into contact with the ground (athirty-fourth invention). And, this applies also to the aforesaidthirty-third invention (a thirty-fifth invention).

This enhances the capability of adjusting floor reaction forces bycorrecting the relative heights of the ground contact portions in theaforesaid base body, making it easier to secure the stability of theposture of a mobile body.

The twenty-second to the thirty-fifth inventions described above may becombined with the third, the fourth, or the seventh to the twenty-firstinventions, as necessary.

In the first, the second, the fifth, and the sixth inventions (or thefirst to the thirty-fifth inventions), a weight has been set on eachnode in the aforesaid tree structure, and

if processing in which, when a predetermined type of state amount, suchas a height or a floor reaction force, is associated with each leaf nodein the aforesaid tree structure, the state amount of each node havingchild nodes is defined as a weighted mean value, which uses theaforesaid weight, of the state amounts of all child nodes of the node, avalue obtained by subtracting a state amount of a parent node of thenode from the state amount of the node is determined as a node relativestate amount of the node on each node except a root node, and zero isdetermined as a relative state amount of the root node, is defined asthe processing for hierarchically relativizing the predetermined type ofstate amount on each node,

if a node relative floor reaction force Fn_rel (n=1, 2, . . . )hierarchically relativized on each node is determined on the basis of afloor reaction force Fn (n=1, 2, . . . ) acting on each of the groundcontact portions, which are the leaf nodes, and when a vector (Fa1_rel,Fa2_rel, . . . , Far_rel) having, as its elements, the node relativefloor reaction forces of all child nodes aj (j=1, 2, . . . , r. rdenotes the total number of the child nodes of an n-th node) of an n-thnode, which is an arbitrary node having child nodes, is represented by alinear linkage of a plurality of predetermined mutually independentvectors R(j) (j=1, 2, . . . , r−1) that are all orthogonal to a vector(Wa1, Wa2, . . . , War) having the weights of all child nodes of then-th node as its elements, a vector having a coefficient of the linearlinkage as its element is defined as a node expansion floor reactionforce moment Mn_exp of the n-th node, and

if a node relative height Zn_rel (n=1, 2, . . . ) hierarchicallyrelativized on each node is determined on the basis of a height Zn (n=1,2, . . . ) of each of the ground contact portions, which are theaforesaid leaf nodes, and when a vector (Za1_rel, Za2_rel, . . . ,Zar_rel) having the node relative heights of all child nodes aj (j=1, 2,. . . , r. r denotes the total number of the child nodes of an n-thnode) of the aforesaid n-th node as its elements is expressed by alinear linkage of the plurality of the predetermined mutuallyindependent vectors R(j) (j=1, 2, . . . , r−1), a vector having acoefficient of the linear linkage as its element is defined as a nodeexpansion inclination angle θn of the n-th node,

then, the aforesaid node operation controlling means may determine theaforesaid corrected desired motion by adding, to the aforesaid desiredmotion, a first correction of the desired relative heights of aplurality of ground contact portions belonging to the C-th node by usingthe aforesaid node expansion floor reaction force moment Mn_exp(n=C) ora node expansion inclination angle θn(n=C) of at least the aforesaidC-th node (a thirty-sixth invention).

Using the node expansion floor reaction force moment or the nodeexpansion inclination angle defined as described above makes it possibleto properly correct the desired relative heights of a plurality ofground contact portions belonging to a C-th node even if there are fouror more ground contact portions that belong to the C-th node.

To be more specific, the thirty-sixth invention comprises a means fordetermining a desired floor reaction force that should act on eachground contact portion belonging to at least the aforesaid C-th node onthe basis of at least the desired floor reaction force of the aforesaiddesired gait,

wherein the aforesaid node operation controlling means comprises a meansfor determining a desired node expansion moment, which is a desiredvalue of the aforesaid node expansion moment of the C-th node, on thebasis of a desired floor reaction force of each ground contact portionbelonging to the aforesaid C-th node, a means for determining an actualnode expansion moment, which is an actual value of the aforesaid nodeexpansion moment of the C-th node on the basis of an actual floorreaction force of each ground contact portion belonging to the aforesaidC-th node, and a means for determining a manipulated variable of theaforesaid node expansion inclination angle of the aforesaid C-th node onthe basis of the aforesaid actual node expansion moment and theaforesaid desired node expansion moment, and determines the aforesaidcorrected desired motion by adding, to the aforesaid desired motion, afirst correction of the desired relative heights of a plurality ofground contact portions belonging to the aforesaid C-th node on thebasis of the aforesaid determined manipulated variable of the nodeexpansion inclination angle (a thirty-seventh invention).

According to the thirty-seventh invention, when the manipulated variableof the aforesaid node expansion inclination angle of the aforesaid C-thnode is determined on the basis of the aforesaid actual node expansionmoment and the aforesaid desired node expansion moment (e.g., in orderto bring their errors close to zero), the manipulated variablerepresents the manipulated variable of the relative heights the childnodes of the C-th node. Thus, based on the manipulated variable of thenode expansion inclination angle, a corrected desired motion can beproperly determined by adding, to a desired motion, the first correctionof the desired relative heights of the plurality of ground contactportions belonging to the C-th node.

Further, the first, the second, the fifth, and the sixth inventionsdescribed above (or the first to the thirty-seventh inventions)preferably comprise a floor configuration estimating means forestimating a parameter that specifies the relative heights of the groundcontact surfaces of a plurality of ground contact portions belonging toat least the aforesaid C-th node as the floor configuration parameterthat represents a floor configuration,

wherein the node operation controlling means determines the aforesaidcorrected desired motion by further adding a third correction of thedesired relative heights of a plurality of the ground contact portionsbelonging to the aforesaid C-th node on the basis of an estimated valueof the aforesaid floor configuration parameter when determining theaforesaid corrected desired motion (a thirty-eighth invention).

According to the thirty-eighth invention, a third correction of thedesired relative heights of a plurality of the ground contact portionsbelonging to the C-th node, which is based on an estimated value of afloor configuration parameter that specifies the relative heights of theground contact surfaces of a plurality of ground contact portionsbelonging to at least the C-th node, is further added to a desiredmotion; therefore, even if an actual floor configuration differs fromthe one assumed in the aforesaid desired motion, it is possible to makethe ground contact surfaces of the ground contact portions properly comein contact with the actual floor surface when they should come incontact with it and consequently to cause a proper floor reaction forceto act on a mobile body. Hence, the stability of a posture of the mobilebody can be further enhanced.

The thirty-eighth invention preferably comprises a means for determiningan actual node floor reaction force, which is the actual value of thenode floor reaction force of each child node of at least the aforesaidC-th node, from the actual floor reaction force of each ground contactportion belonging to the C-th node,

wherein the aforesaid floor configuration estimating means performsestimation while sequentially updating the aforesaid floor configurationparameter on the basis of a past value of an estimated value of theaforesaid floor configuration parameter, a difference between theaforesaid corrected desired motion and the aforesaid desired motion, theaforesaid corrected motion, at least either a detected value or anestimated value of an actual joint displacement, which is a displacementamount of each joint of the aforesaid mobile body, the aforesaid actualposture inclination, and a relative relationship among the actual nodefloor reaction forces of child nodes of at least the aforesaid C-th node(a thirty-ninth invention).

Alternatively, when the aforesaid ground contact portions are connectedto the base body of the aforesaid mobile body through the intermediaryof connecting mechanisms such that they are movable with respect to thebase body, preferably, the thirty-ninth invention comprises adeformation amount detecting means for detecting or estimating theamounts of deformations taking place in the connecting mechanismsbetween the base body and the ground contact portions belonging to theaforesaid C-th node and in the ground contact portions,

wherein the aforesaid floor configuration estimating means performsestimation while sequentially updating the aforesaid floor configurationparameter on the basis of a past value or an estimated value of theaforesaid floor configuration parameter, a difference between theaforesaid corrected desired motion and the aforesaid desired motion, theaforesaid corrected motion, at least either a detected value or anestimated value of an actual joint displacement, which is a displacementamount of each joint of the aforesaid mobile body, the aforesaid actualposture inclination, and the aforesaid deformation amounts associatedwith the ground contact portions belonging to at least the aforesaidC-th node (a fortieth invention).

According to the thirty-ninth invention and the fortieth invention, itis possible to properly perform the estimation while sequentiallyupdating the aforesaid floor configuration parameter in real time.Supplementally, in the fortieth invention, it is not necessarilyrequired that all of the aforesaid connecting mechanisms be providedwith joints.

In the fortieth invention, the aforesaid deformation amount detectingmeans, for example, estimates the aforesaid deformation amounts on thebasis of the actual floor reaction forces of ground contact portions (aforty-first invention).

Detecting or estimating actual floor reaction forces as described aboveand using them make it possible to properly estimate the deformationamounts of the aforesaid connecting mechanisms and ground contactportions.

In the aforesaid thirty-eighth invention (or the thirty-eighth to theforty-first inventions), a weight of each node in the aforesaid treestructure has been set, and relative to a B-th node, which is each nodein the tree structure, if the B-th node is the aforesaid leaf node, thenthe height of the ground contact surface of a ground contact portionthat is the leaf node is defined as the height of the ground contactsurface of the B-th node, and if the B-th node has child nodes, then aweighted mean value, which uses the aforesaid weight, of the heights ofthe ground contact surfaces of all child nodes of the B-th node isdefined as the height of the ground contact surface of the B-th node,and

the aforesaid floor configuration parameter estimated by the aforesaidfloor configuration estimating means is preferably a parameter thatspecifies the relative relationship among the heights of the groundcontact surfaces of a plurality of child nodes of the aforesaid C-thnode (a forty-second invention).

According to the forty-second invention, the aforesaid floorconfiguration parameter related to the configuration of a floor withwhich the plurality of ground contact portions belonging to the C-thnode comes in contact can be determined on the basis of the relativerelationship of the heights of the ground contact surfaces of theplurality of descendant nodes of the aforesaid C-th node by defining theheight of the ground contact surface of the B-th node having child nodesas the weighted mean value of the child nodes, as described above. Thus,a floor configuration parameter can be estimated without grasping theabsolute height of each ground contact portion. This permits easierfloor configuration estimation processing.

In this forty-second invention, when at least one of the child nodes ofthe aforesaid C-th node is likely to float, the aforesaid floorconfiguration estimating means preferably estimates the aforesaid floorconfiguration parameter that specifies the relative relationship amongthe heights of the ground contact surfaces of the aforesaid plurality ofchild nodes, while maintaining the relative height of the ground contactsurface of that child node, which is likely to float, at a fixed value(a forty-third invention).

More specifically, if at least one of the child nodes of the aforesaidC-th node is likely to float, this means that the ground contact surfaceof that child node has left or is about to leave an actual floorsurface, so that the height of the ground contact surface of that childnode does not reflect the height of the actual floor surface. Hence, insuch a situation, the twenty-second invention estimates the aforesaidfloor configuration parameter that specifies the relative relationshipamong the heights of the ground contact surfaces of the aforesaidplurality of child nodes while retaining the relative height of theground contact surface of the child node, which is about to float, at afixed value. This makes it possible to prevent a floor configurationparameter from diverting to a value that does not reflect an actualfloor surface configuration and makes it possible to estimate a floorconfiguration parameter that reflects an actual floor surfaceconfiguration. This consequently makes it possible to prevent therelative heights of ground contact portions from being corrected byusing inappropriate floor configuration parameters, causing thestability of a posture of a mobile body to be impaired.

Further, in the aforesaid thirty-eighth invention (or the thirty-eighthto the forty-third inventions), the aforesaid floor configurationestimating means preferably estimates the aforesaid floor configurationparameter by using a low-pass filter so as to alleviate fluctuations inthe floor configuration parameter (a forty-fourth invention). Using alow-pass filter as described above makes it possible to prevent a floorconfiguration parameter from frequently changing due to vibrations orthe like when a mobile body travels.

Further, in the aforesaid thirty-eighth invention (or the thirty-eighthto the forty-fourth inventions), a weight has been set on each node inthe aforesaid tree structure, and

if processing in which, when a predetermined type of state amount, suchas a height or a floor reaction force, is associated with each leaf nodein the aforesaid tree structure, the state amount of each node havingchild nodes takes a weighted mean value, which uses the aforesaidweight, of the state amounts of all child nodes of the node, a valueobtained by subtracting a state amount of a parent node of each nodefrom the state amount of the node is determined as a node relative stateamount of the node on each node except a root node, and zero isdetermined as a relative state amount of the root node, is defined asthe processing for hierarchically relativizing the predetermined type ofstate amount on each node,

if a node relative floor reaction force Fn_rel (n=1, 2, . . . )hierarchically relativized on each node is determined on the basis of afloor reaction force Fn (n=1, 2, . . . ) acting on each of the groundcontact portions, which are the aforesaid leaf nodes, and if a vector(Fa1_rel, Fa2_rel, . . . , Far_rel) having, as its elements, the noderelative floor reaction forces of all child nodes aj (j=1, 2, . . . , r.r denotes the total number of the child nodes of an n-th node) of ann-th node, which is an arbitrary node having child nodes, is representedby a linear linkage of a plurality of predetermined mutually independentvectors R(j) (j=1, 2, . . . , r−1) that are all orthogonal to a vector(Wa1, Wa2, . . . , War) having the weights of all child nodes of then-th node as its elements, then a vector having a coefficient of thelinear linkage as its element is defined as a node expansion floorreaction force moment Mn_exp of the n-th node, and if a node relativeheight Zn_rel (n=1, 2, . . . ) hierarchically relativized on each nodeis determined on the basis of a height Zn (n=1, 2, . . . ) of the groundcontact surface of each of the ground contact portions, which are theaforesaid leaf nodes, and if a vector (Za1_rel, Za2_rel, . . . ,Zar_rel) having the node relative heights of all child nodes aj (j=1, 2,. . . , r. r denotes the total number of the child nodes of an n-thnode) of the aforesaid n-th node as its elements is expressed by alinear linkage of the plurality of the aforesaid predetermined mutuallyindependent vectors R(j) (j=1, 2, . . . , r−1), then a vector having acoefficient of the linear linkage as its element is defined as a nodeexpansion inclination angle θn of the n-th node, and

the floor configuration estimating means may estimate the aforesaidfloor configuration parameter by using the aforesaid node expansionfloor reaction force moment Mn_exp(n=C) or the node expansioninclination angle θn(n=C) of at least the aforesaid C-th node (aforty-fifth invention).

In this forty-fifth invention, relative to a B-th node, which is eachnode in the aforesaid tree structure, if the B-th node is the aforesaidleaf node, then the height of the ground contact surface of the groundcontact portion that is the leaf node is defined as the height of theground contact surface of the B-th node, and if the B-th node has childnodes, then a weighted mean value, which uses the aforesaid weight, ofthe heights of the ground contact surfaces of all child nodes of theB-th node, is defined as the height of the ground contact surface of theB-th node, and

the aforesaid floor configuration parameter preferably includes aparameter that uses the aforesaid node expansion inclination angle toindicate the relative height of the ground contact surface of each childnode of the aforesaid C-th node (a forty-sixth invention).

With this arrangement, a floor configuration parameter can be properlyestimated by using the aforesaid node expansion inclination angle evenif there are four or more ground contact portions belonging to the C-thnode.

Further, the aforesaid forty-fifth invention preferably comprises ameans for determining a desired floor reaction force that should act oneach ground contact portion belonging to at least the aforesaid C-thnode on the basis of at least the desired floor reaction force of theaforesaid desired gait,

wherein the aforesaid node operation controlling means comprises a meansfor determining a desired node expansion moment, which is a desiredvalue of the aforesaid node expansion moment of the C-th node, on thebasis of the desired floor reaction force of each ground contact portionbelonging to the aforesaid C-th node, a means for determining an actualnode expansion moment, which is the actual value of the aforesaid nodeexpansion moment of the C-th node on the basis of the actual floorreaction force of each ground contact portion belonging to the aforesaidC-th node, and a means for determining a manipulated variable of theaforesaid node expansion inclination angle of the aforesaid C-th node onthe basis of the aforesaid actual node expansion moment and theaforesaid desired node expansion moment, and determines the aforesaidcorrected desired motion by adding, to the aforesaid desired motion, afirst correction of the desired relative heights of a plurality ofground contact portions belonging to the aforesaid C-th node on thebasis of the aforesaid determined manipulated variable of the nodeexpansion inclination angle (a forty-seventh invention).

With this arrangement, even if there are four or more ground contactportions belonging to a C-th node, a floor configuration parameter canbe estimated while properly controlling a floor reaction force by usinga node expansion inclination angle and a node expansion moment.

Incidentally, in the first to the forty-seventh inventions describedabove, “floor reaction force” is to include a reaction force receivedfrom an object other than a floor with which a mobile body comes incontact rather than simply meaning a reaction force acting on the mobilebody from a floor (or the ground), as in its original meaning. Further,the ground contact portion is to include not only a portion that comesin contact with a floor (or the ground) but also a portion of the mobilebody that comes in contact with an object.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is an overall external view of a robot in accordance with a firstembodiment and a second embodiment of the present invention,

FIG. 2 is a block diagram showing the functional construction of acontrol device of the first embodiment,

FIGS. 3( a) to (c) and

FIGS. 4( a) to (c) are diagrams for explaining an operation of the robot(four-legged robot) of the first embodiment,

FIGS. 5( a) to (c) are graphs showing weight setting examples in thefirst embodiment, and

FIG. 5( d) is a graph showing a setting example of a ZMP (desired totalfloor reaction force central point) in the first embodiment.

FIG. 6 is a diagram showing the hierarchical structure of nodes in thefirst embodiment,

FIG. 7 is a diagram showing a relationship between desired node floorreaction force central points and weights,

FIG. 8 is a diagram showing a relationship between desired node floorreaction forces and weights, and

FIG. 9 is a flowchart showing a main routine processing of the controldevice of the first embodiment.

FIG. 10 to FIG. 14 are diagrams illustrating desired node floor reactionforce translational force components, actual node floor reaction forcetranslational force components, actual node floor reaction forcemoments, compensating total floor reaction force moments, and nodecompensating floor reaction force moments, respectively, of a robot(four-legged robot) of the first embodiment.

FIG. 15 and FIG. 16 are diagrams for explaining position corrections bynode compensating angles (compliance operation) in the first embodiment,

FIG. 17 to FIG. 21 are block diagrams respectively showing thefunctional constructions of a hierarchical compliance operationdeterminer, a compensating total floor reaction force momentdistributor, a θ 1423 determiner (compensating angle determiner), a θ 14determiner (compensating angle determiner), and a mechanism deformationcompensation amount calculator in the first embodiment,

FIG. 22 is a flowchart showing the processing for determiningcompensating angles, and

FIG. 23 is a diagram for explaining another example of a hierarchicalstructure related to the robot according to the first embodiment.

FIG. 24 is a diagram for explaining the hierarchical structure of arobot (six-legged robot) of a second embodiment,

FIG. 25 is a block diagram showing the functional construction of ahierarchical compliance operation determiner in the second embodiment,

FIG. 26 to FIG. 28 are diagrams respectively illustrating thetranslational force components of desired node floor reaction forces,the translational force components of actual node floor reaction forces,and actual node floor reaction force moments of the robot (six-leggedrobot) of the second embodiment,

FIGS. 29( a) and (b) are diagrams for explaining the technique forsetting a node compensating floor reaction force moment in the secondembodiment, and

FIG. 30 and FIG. 31 are diagrams for explaining a position correctionbased on node compensating angles (compliance operation) in the secondembodiment.

FIG. 32 and FIG. 33 are block diagrams respectively showing thefunctions of a θ 145236 determiner (compensating angle determiner) and aθ 145 determiner (compensating angle determiner) in the secondembodiment, and

FIG. 34 is a diagram for explaining another example of a hierarchicalstructure related to the robot of the second embodiment.

FIG. 35 is a block diagram showing the functional construction of ahierarchical compliance operation determiner in a third embodiment,

FIG. 36 is a flowchart showing a main routine processing of a controldevice in the third embodiment,

FIG. 37 to FIG. 39 are diagrams for explaining concepts and terms in thethird embodiment,

FIG. 40 is a block diagram showing the function of a floor configurationestimator in the third embodiment, and

FIG. 41 is a flowchart showing the processing of a floor height errorestimation processing subroutine in FIG. 40.

FIG. 42 and FIG. 43 are diagrams for explaining the processing forhierarchical relativization in a four-legged robot and a six-leggedrobot, respectively.

FIG. 44 is a diagram showing examples of setting request modes of nodesrelated to estimation of a floor configuration, and

FIG. 45 to FIG. 55 are flowcharts showing the processing for estimatinga floor configuration.

FIG. 56 is a block diagram showing the functions of a floorconfiguration estimator in a fourth embodiment, and

FIG. 57 is a block diagram showing the functions of a floorconfiguration estimator in a fifth embodiment.

FIG. 58 and FIG. 59 are diagrams showing a state wherein a robot(two-legged mobile robot) in a sixth embodiment is kneeling,

FIG. 60 is a diagram showing the construction of a floor reaction forcesensor of a knee of the robot in the sixth embodiment,

FIG. 61 is a diagram showing a hierarchical structure of nodes in thesixth embodiment, and

FIG. 62 is a block diagram showing the functional construction of ahierarchical compliance operation determiner in the sixth embodiment.

FIGS. 63( a) to (c) are diagrams for explaining a technique forcorrecting the posture of the body of the robot in the sixth embodiment,

FIG. 64 is a diagram for explaining a technique for correcting theposition/posture of the body of the robot in the sixth embodiment, and

FIG. 65 is a block diagram showing the functions of an inversekinematics calculator in the sixth embodiment.

FIG. 66 is a block diagram showing the functions of an inversekinematics calculator in a seventh embodiment, and

FIG. 67 and FIG. 68 are diagrams for explaining a technique forcorrecting the posture of a robot in the seventh embodiment.

FIG. 69 is a diagram showing the construction of a robot in an eighthembodiment and

-   -   FIG. 70 is a diagram showing a hierarchical structure of nodes        in the eighth embodiment.

BEST MODE FOR CARRYING OUT THE INVENTION

First, major terms to be used in the explanation of the embodiments ofthe present invention will be explained.

In the technology of the composite-compliance control disclosed inJapanese Unexamined Patent Application Publication No. H10-277969previously proposed by the present applicant, floor reaction forces aresorted for each leg on the assumption that floor reaction forces arereceived at the distal portion (foot) of a leg of a robot. In comparisontherewith, according to the embodiments of the present description,situations are also considered in which floor reaction forces arereceived at portions other than the distal portions of legs, e.g., asituation in which a human type robot is kneeling, sitting on a chair,or has its arm in contact with the ground. For this reason, a portion ofa legged mobile robot that comes in contact with a floor and receives areaction force in a predetermined operation of the robot is referred toas “the ground contact portion.” In this case, “floor” does not simplyrefer to a floor (or the ground) in a conventional sense, but “floor”also includes an object with which the robot comes in contact andreceives a reaction force during its movement, such as a chair (a chairon which the robot sits) fixed to the floor (or the ground). Thus, if,for example, a two-legged mobile robot sits on a chair or the like, thenthe waist of the robot will correspond to the ground contact portion. Ina normal travel of a legged mobile robot (in the walking or the like ofthe two-legged mobile robot), the distal portion (foot or the like) ofeach leg will of course correspond to the ground contact portion.

To categorize (discriminate) ground contact portions, even if groundcontact portions are distributed such that they are separated at aplurality of locations on the same link (a part corresponding to asingle rigid body)(if a plurality of portions separated from each otheron the same link is in contact with the ground), that is, even if aplurality of ground contact surfaces is not connected on the same link,these will be put together and defined as one ground contact portion.For instance, if the ground contact portion provided with four spikepins comes in contact with the ground through the intermediary of thespike pins, then the individual ground contact surfaces of the fourspike pins will be put together and regarded as one ground contactportion. However, it is not always required to put the ground contactsurfaces together into one.

An n-th ground contact portion is a designation for identifying theground contact portion which has been classified (discriminated)according to such a rule and to which an identification number n (n=1,2, 3, . . . ) has been assigned.

A ground contact portion floor reaction force refers to a floor reactionforce acting on the ground contact portion, and the floor reaction forceacting on an n-th ground contact portion, in particular, is referred toas the n-th ground contact portion floor reaction force. The total floorreaction force is the resultant force of the floor reaction forcesacting on all ground contact portions. The floor reaction force centralpoint is the point of action at which the horizontal component of amoment generated by a floor reaction force at that point is zero.

A floor reaction force, such as the ground contact portion floorreaction force or a total floor reaction force, is usually expressed bya set of the point of action of a force and a translational force andmoment applied to that point of action. For the same floor reactionforce, there are a limitless number of sets of point of action,translational force and moment for expressing the floor reaction force.Among them, there is an expression of a floor reaction force that uses,as the point of action, a point at which a moment component (thehorizontal component of a moment) except a component about a verticalaxis (vertical component) is zero. The point of action in thisexpression is referred to as a floor reaction force central point in thepresent description. A floor reaction force central point may be definedas a point at which the floor surface parallel component (the componentparallel to a floor surface) of a moment generated by a floor reactionforce at that point is zero.

In the present description, “floor surface” may alternatively be avirtual floor surface (a floor surface assumed in a desired gait and itdoes not necessarily coincide with an actual floor surface) described inJapanese Unexamined Patent Application Publication No. H5-318340previously proposed by the present applicant.

Supplementally, the floor reaction force central point of the groundcontact portion is usually set on the ground contact surface thereof(the surface in contact with a floor) if the ground contact portion isin contact with the ground. Meanwhile, the ground contact portion floorreaction force while the ground contact portion is moving in the air isalways zero, so that a moment horizontal component of the ground contactportion floor reaction force will be zero at any point of action. Thismeans that the floor reaction force central point can be arbitrarilyset. However, to smoothly control the operation of a robot, a desiredfloor reaction force central point desirably moves in continuity. Hence,according to the embodiments in the present description, a floorreaction force central point of the ground contact portion floorreaction force is defined as the point of action that moves togetherwith the ground contact portion when the ground contact portion moves inthe air.

According to the embodiments in the present description, in thecompliance control operation in which the position/posture of eachground contact portion are corrected on the basis of at least an actualfloor reaction force (a floor reaction force actually acting on arobot), the compliance control operation is not simply performed on eachof ground contact portions irrelevantly (independently) from each other.The embodiments in the present description are characterized in thatground contact portion are sorted by a tree structure and operations forcorrecting the positions/postures of ground contact portions aredetermined on the basis of at least an actual floor reaction forceacting on each group that has been sorted (the actual floor reactionforces or the resultant force thereof acting on all ground contactportions included in each group). Hereinafter, classification by thetree structure may be referred to as “hierarchization.”

For the desired values of variables generated by a gait generatingdevice of a legged mobile robot in the embodiments in the presentdescription, “desired” will be attached to the beginning of the names ofvariables. For the values of the variables in an actual legged mobilerobot (actually detected values or estimated values, because true valuesare unknown), “actual” will be attached to the beginning of the names ofvariables. The above “actual floor reaction force” is an examplethereof.

The targets of the total floor reaction forces (the resultant force ofthe actual floor reaction forces acting on all ground contact portionsof the robot) in the compliance control (floor reaction force control)to be discussed later will be referred to as desired total floorreaction forces. The point at which the moment horizontal component of adesired total floor reaction force is zero will be referred to as adesired total floor reaction force central point.

The point on a floor surface at which the moment horizontal component ofthe resultant force of an inertial force generated by a motion of adesired gait of a mobile robot (the motion of each portion of the robotin a desired gait) and the gravity acting on the robot is zero will bereferred to as a desired ZMP. The motion of a desired gait is determinedby the time series of the desired position/posture of each portion ofthe robot in the desired gait, so that the time series of the desiredposition/posture of the portion will be generically referred to as amotion or a desired motion of a desired gait. Supplementally, if thesetime series can be specified, then desired motions may be described byexpressions that are different from the desired motions defined asdescribed above. For example, a set of the time series of the desireddisplacements of joints of a robot and the time series of the desiredposition/posture of a particular portion, such as a base body, may beused as a desired motion.

A desired total floor reaction force is usually a total floor reactionforce that dynamically balances with the motion pattern of a desiredgait (the time-series pattern of a desired motion). Hence, a desiredtotal floor reaction force central point usually agrees with a desiredZMP. Thus, in the following explanation, a desired total floor reactionforce central point and a desired ZMP will be used withoutdiscriminating them in many cases. Exceptionally, however, in the caseof controlling a robot explained in, for example, Japanese Patent No.3269852 previously proposed by the present applicant, a desired totalfloor reaction force central point and a desired ZMP do not necessarilyagree with each other. In the following explanation, the term, desiredZMP, will be used in some cases, but there will be some places where theterm, desired total floor reaction force central point, should be usedto be precise.

It may be assumed that, in a desired gait of a mobile robot, the robotis subject to a reaction force (an external force) other than a floorreaction force from an environment. And, the reaction force (theexternal force) other than a floor reaction force may be referred to as,for example, a desired object reaction force, and the definition of adesired ZMP described above may be expanded as follows. The resultantforce of the inertial force generated by a motion pattern of a desiredgait of a robot, the gravity acting on the robot, and a desired objectreaction force may be dynamically determined, and if the momentgenerated at a certain point on a floor surface by the resultant forceis zero except for a component about a vertical axis, then the point maybe defined anew as a desired ZMP. However, if a desired object reactionforce is taken as one form of a floor reaction force, then thedefinition of a desired ZMP may be the same definition previouslydescribed.

Based on the above, the embodiments of the present invention will beexplained in detail below.

First Embodiment

FIG. 1 is an external view of a general multi-legged mobile robot(legged mobile robot) according to first and second embodiments. FIG. 1shows that the robot 1 has six legs (leg bodies), namely, a first leg #1to a sixth leg #6; however, the robot in the first embodiment does nothave the fifth leg #5 and the sixth leg #6. This means that, in thefirst embodiment, the robot 1 is a four-legged robot having four legs(leg bodies), the first leg #1 to the fourth leg #4. In FIG. 1, thecomponents of the robot 1 according to the second embodiment are shownby parenthesized reference numerals.

As shown in FIG. 1, in the robot 1 (the four-legged robot) according tothe first embodiment, two legs (the first leg #1 and the third leg #3)are extended from the right side of a body 24, which is the base body ofthe robot 1, such that they are arranged side by side in thelongitudinal direction, and in the same manner, two legs (the second leg#2 and the fourth leg #4) are extended from the left side of the body 24such that they are arranged side by side in the longitudinal direction.A discoid ground contact portion 10 is attached to the distal portion ofeach of the legs #1 to #4 through the intermediary of a spherical joint12, which is a free joint. Hereinafter, when the ground contact portions10 need to be discriminated among the legs #1 to #4, they will bereferred to as the first ground contact portion, the second groundcontact portion, the third ground contact portion, and the fourth groundcontact portion.

Each ground contact portion 10 is engaged with the spherical joint 12such that its central point agrees with the central point of thespherical joint 12 and that a floor reaction force moment (the momentcomponent of a floor reaction force) will not act on the central pointof the ground contact portion 10 (strictly speaking, the spherical joint12). This means that the floor reaction force moment (actual floorreaction force moment) at the central point of the ground contactportion 10 will be always zero.

In the robot 1 shown in the figure, each of the legs #1 to #4 isprovided with joints 14 and 15 at a portion adjacent to the body 24 ofthe robot 1 and at an intermediate portion, respectively, and acompliance mechanism 42 composed of an elastic member, such as a spring,and a six-axis force sensor 34 serving as a floor reaction forcedetecting means for detecting an actual floor reaction force acting onthe ground contact portion 10 are provided in the vicinity of the distalportion of each of the legs #1 to #4 (a portion of the link connectingthe spherical joint 12 and the joint 14, the portion being adjacent tothe spherical joint 12). In the example shown in the figure, the joints14 permit rotation about two axes, while the joints 15 permit rotationabout one axis. The bottom surfaces of the ground contact portions maybe provided with elastic members made of sponge, rubber or the likeserving as compliance mechanisms.

The displacement operation (the rotational operation about each axis) ofeach of the joints 14 and 15 is performed by an actuator, such as anelectric motor, which is not shown. And, an actual joint displacement,which is the actual displacement amount (the angle of rotation abouteach axis), of each of the joints 14 and 15 is detected by a sensor,such as a rotary encoder, which is not shown. The six-axis force sensor34 is capable of detecting the translational forces in the directions ofthree axes and the moments about three axes; however, in the robot 1 inthe first embodiment, no actual floor reaction force moment acts on thecentral points of the ground contact portions 10, as described above.Hence, three-axis force sensors that detect the translational forces inthe directions of three axes or force sensors that detect only thevertical components of the translational forces of actual floor reactionforces may be used in place of the six-axis force sensors 34.

The body 24 incorporates a control device 50 constructed of anelectronic circuit unit that includes a microcomputer, an actuator drivecircuit, etc., a posture sensor 36 for detecting the postures of thebody 24, and a power source not shown (a secondary battery, a capacitor,or the like). The posture sensor 36 is constructed of, for example, anacceleration sensor and a gyro sensor. In the present description,“posture” generally means a spatial orientation (however, “the posture”of the entire robot means an instantaneous value of a motion of therobot). Further, in the present embodiment, the posture sensor 36detects the posture inclinations (inclination angles) of the postures ofthe body 24 in, for example, the pitch directions and roll directions inrelation to the vertical direction. This means that the posture sensor36 functions as an actual posture inclination detecting means fordetecting actual posture inclinations of the body 24.

If the postures (rotational angles) of the body 24 of the robot 1 in theyaw direction are to be also controlled, then the rotational angles ofthe body 24 in the yaw direction (in other words, the postures of thebody 24 in the directions of three axes) may be also detected by theposture sensor 36.

FIG. 2 is a block diagram showing the functional construction andoperations of the control device 50. The actual robot 1 shown in FIG. 2is the robot 1 shown in FIG. 1 from which the control device 50, theposture sensor 36, and the six-axis force sensor 34 have been removed.Here, a predetermined coordinate system (XYZ coordinate system) fixed toa floor, in which the approximately front direction of the robot 1 isdefined as X axis, approximately left side direction thereof is definedas Y axis, and the upward direction thereof is defined as Z axis, asshown in FIG. 1, is referred to as “a supporting leg coordinate system”or “a global coordinate system.” Hereinafter, positions, postures,translational forces and moments will be expressed in terms ofcoordinate components of the supporting leg coordinate system (theglobal coordinate system) unless otherwise specified. The origin of thesupporting leg coordinate system (global coordinate system) does nothave to be steadily fixed at a single point; the position of the originwith respect to a floor may be changed while the robot 1 is traveling.For example, the position of the origin of the supporting leg coordinatesystem (the global coordinate system) may be changed each time apredetermined leg of the robot 1 lands.

As shown in FIG. 2, the control device 50 is provided with a gaitgenerating device 100, a desired floor reaction force distributor 102, arobot geometric model (inverse kinematics calculator) 110, ahierarchical compliance operation determiner 114, a displacementcontroller 112, an actual floor reaction force detector 108, a postureerror calculator 103, and a posture stabilization control calculator 104as its functional components (functional means). The following willexplain the overviews of the components of the control device 50.

The gait generating device 100 has a function as a desired gaitdetermining means or a desired motion determining means in the presentinvention, and generates (determines) and outputs a desired gait thatspecifies an operation of the robot 1. In the present embodiment, adesired gait is formed of the trajectory of a desired motion of therobot (the time series of a desired position/posture of each portion ofthe robot) and the trajectory of a desired floor reaction force (thetime series of a set of the desired position of the action point of areaction force received by the robot from a floor and the desired valuesof a translational force and moment applied to the action point). In thepresent description, “trajectory” means a time-series pattern (temporalchange pattern).

The trajectory of a desired motion output by the gait generating device100 is constructed of a desired ground contact portion trajectory, whichis the trajectory of the desired values of the position and the postureof each ground contact portion 10 of the robot 1, and a desired bodyposition/posture trajectory, which is the trajectory of the desiredvalues of the position and the posture of the body 24 of the robot 1. Agait generating device in a robot equipped with joints related to armsor a head, as in a sixth embodiment to be discussed later, determinesand outputs the desired position/posture trajectories of the arms or thehead as constituents of desired motions.

Further, in the present embodiment, the trajectory of a desired floorreaction force output by the gait generating device 100 is formed of adesired total floor reaction force central point trajectory, which isthe trajectory of a desired position of the total floor reaction forcecentral point of the robot 1, and a desired total floor reaction forcetrajectory, which is the trajectory of a desired value of a total floorreaction force that uses the above desired total floor reaction forcecentral point as an action point. In the present embodiment, a desiredtotal floor reaction force central point trajectory is regarded asidentical to a desired ZMP trajectory, which is the trajectory of adesired position of ZMP.

The position of each ground contact portion 10 is the position of acertain representative point of the ground contact portion 10, and therepresentative point is, for example, the projected point obtained byprojecting, in the vertical direction, the central point of each groundcontact portion 10 (the central point of the spherical joint 12) ontothe ground contact surface (bottom surface) of the ground contactportion 10, or the central point of the ground contact portion 10 (thecentral point of the spherical joint 12). Hereinafter, the position ofthe representative point of each ground contact portion 10 will bereferred to simply as the ground contact portion position. Thetrajectory of a desired value of the ground contact portion position (adesired ground contact portion position trajectory) is included in theaforesaid desired ground contact portion trajectory determined by thegait generating device 100.

Here, in the robot 1 of the present embodiment, the ground contactportions 10 are engaged with the spherical joints 12, which are freejoints, so that the postures of the ground contact portions 10 cannot becontrolled. Therefore, according to the present embodiment, the gaitgenerating device 100 does not actually generate the trajectories ofdesired postures of the ground contact portions 10 (does not have togenerate them). In the present embodiment, therefore, the desired groundcontact portion trajectory means the same as a desired ground contactportion position trajectory.

However, if the ground contact portions are attached to the distalportions of the legs such that their postures can be controlled (if theground contact portions are attached to the distal portions of the legsthrough joints that can be operated by actuators), as in the robot ofthe sixth embodiment to be discussed later, then the trajectories ofdesired postures of the ground contact portions should be included inthe desired ground contact portion trajectories. In the presentdescription, the term “ground contact portion position/posture” will befrequently used to generally consider the aforesaid case, the termsubstantially meaning “ground contact portion position” in the presentembodiment.

Referring to FIG. 3( a) to FIG. 3( c) and FIG. 4( a) to FIG. 4( c), adesired ground contact portion trajectory (a desired ground contactportion position trajectory) and a desired total floor reaction forcecentral point trajectory will be explained more specifically. Accordingto the first embodiment, a travel of the robot 1 is accomplished bycarrying out the motions of the legs #1 to #4 by causing a pair of legs,which are to be free legs, to leave a floor and move in the air and thenland at desired positions by repeating, in order, a period during whichthe pair of the first leg #1 and the fourth leg #4 out of the legs #1 to#4 is a pair of supporting leg, while the pair of the second leg #2 andthe third leg #3 is a pair of free legs, a period during which all legs#1 to #4 are supporting legs, and a period during which the pair of thefirst leg #1 and the fourth leg #4 is a pair of free legs, while thepair of the second leg #2 and the third leg #3 is a pair of supportinglegs. A supporting leg is a leg that is in contact with the ground tosupport the self-weight of the robot 1 (a leg to be subject to a floorreaction force that is not zero), and a free leg is a non-supportingleg.

FIG. 3( a) to FIG. 3( c) and FIG. 4( a) to FIG. 4( c) show the desiredground contact portion positions (more specifically, the positions on ahorizontal plane (XY plane)) of the distal ends of the legs #1 to #4during the aforesaid travel of the robot 1 in time series in sequence(in the sequence of time t1 to t6). The triangles corresponding toreference characters Q1 to Q4 in these figures indicate the desiredground contact portion positions of the first to the fourth groundcontact portions 10, respectively (the positions of the aforesaidrepresentative points of the first to the fourth ground contact portions10 on a horizontal plane (XY plane)).

Supplementally, to be precise, the triangles with the referencecharacters Q1 to Q4 in FIG. 3 and FIG. 4 indicate the positions ofdesired node floor reaction force central points (desired ground contactportion floor reaction force central points) to be discussed later,which are related to the ground contact portions 10. In the presentembodiment, however, the representative points of the ground contactportions 10 are set as described above, so that the desired groundcontact portion positions (the desired positions of the representativepoints) of the first to the fourth ground contact portions 10 agree withthe positions of the desired node floor reaction force central points Qn(n=1, 2, 3, 4), which will be discussed later, or have a certainpositional relationship with the positions of the respective desirednode floor reaction force central points Qn (n=1, 2, 3, 4) (thepositions on the horizontal plane (XY plane) agree). For this reason,the triangles corresponding to the reference characters Q1, Q2, Q3 andQ4 in FIG. 3 and FIG. 4 denote the desired node floor reaction forcecentral points, which will be discussed later, of the first to thefourth ground contact portions 10, respectively, and also denote thepositions of the first to the fourth ground contact portions 10,respectively.

FIG. 3( a) shows the desired ground contact portion positions of theground contact portions 10 at the instant (time t1) when the pair of thefirst leg #1 and the fourth leg #4 as free legs is landed, the pair ofthe second leg #2 and the third leg #3 being supporting legs, FIG. 3( b)shows the desired ground contact portion positions at time t2 in a statewherein all the legs #1 to #4 are supporting legs, and FIG. 3( c) showsthe desired ground contact portion positions at time t3 immediatelybefore the pair of the second leg #2 and the third leg #3 as free legsis separated from a floor (lifted into the air), the pair of the firstleg #1 and the fourth leg #4 being supporting legs. Further, FIG. 4( a)shows the desired ground contact portion positions at time t4 in a statewherein the pair of the second leg #2 and the third leg #3 as free legsis lifted into the air, the pair of the first leg #1 and the fourth leg#4 being supporting legs, FIG. 4( b) shows the desired ground contactportion positions at the instant (time t5) when the pair of the secondleg #2 and the third leg #3 as free legs is landed, the pair of thefirst leg #1 and the fourth leg #4 being supporting legs, and FIG. 4( c)shows the desired ground contact portion positions at time t6immediately before the pair of the first leg #1 and the fourth leg #4 asfree legs is separated from a floor, the pair of the second leg #2 andthe third leg #3 being supporting legs. Incidentally, in FIG. 4( a), thedesired ground contact portion positions of the second leg #2 and thethird leg #3, which are free legs, are indicated by dashed-linetriangles. Supplementally, the trajectory of the positions of the groundcontact portions 10 of free legs in the vertical direction (the Z-axisdirection) is determined such that they rise from a floor surface to apredetermined height and then lower to land again.

Points P in these FIG. 3 and FIG. 4 indicate desired total floorreaction force central points (desired ZMPs). The desired total floorreaction force central point trajectories are determined such that theycontinuously move while existing in a range in which ZMPs may exist (aregion on a floor surface corresponding to a so-called supportingpolygon) at positions not excessively close to the boundaries of therange (e.g., at an approximately central position of the range wherein aZMP may exist). In the first embodiment, when two legs #1 and #4 or #2and #3 are supporting legs (refer to FIGS. 3( a) and (c) and FIGS. 4( a)to (c)), a desired total floor reaction force central point is set on asegment connecting the representative points of the ground contactportions 10 and 10 of those legs such that it is not excessively closeto an end point of the segment. When all legs #1 to #4 are supportinglegs (refer to FIG. 3( b)), a desired total floor reaction force centralpoint is set within a polygon having the representative points of allground contact portions 10 as its apex angles such that it is notexcessively close to a boundary of the polygon. FIG. 5( d) is a graphillustrating a trajectory of a component ZMPx in the X-axis direction(in the direction in which the robot 1 advances) of the position of thedesired total floor reaction force central point (desired ZMP)determined as described above. FIGS. 5( a) to (c) are graphsillustrating setting examples of weight, which will be discussed later.

Further, a desired body position/posture trajectory determined by thegait generating device 100 is determined using a dynamic model or thelike of the robot 1 such that the horizontal component of a momentgenerated about a desired ZMP by the resultant force of the inertialforce generated by a desired motion of the robot 1 and the gravityacting on the robot 1 becomes zero. Incidentally, “the body position” isthe position of a certain representative point of the body 24.

A desired total floor reaction force determined by the gait generatingdevice 100 is constructed of the desired values of the translationalforce and the moment acting on a desired total floor reaction forcecentral point, and it is determined such that it balances with theresultant force of the inertial force generated by a desired motion ofthe robot 1 and the gravity acting on the robot 1 at a desired totalfloor reaction force central point in the present embodiment. The momenthorizontal component of the desired total floor reaction force about thedesired total floor reaction force central point (desired ZMP) is zero.Supplementally, it is not necessary to determine all components of thetranslational force and the moment acting on the desired total floorreaction force central point as desired values. For instance, if theposture or the floor reaction force of the robot 1 about the verticalaxis is not controlled, then it is not necessary to determine thecomponent of the moment of the desired total floor reaction force aboutthe vertical axis (the component in the Z-axis direction).

The desired ground contact portion trajectory determined by the gaitgenerating device 100 (the desired ground contact portion positiontrajectory) is corrected by a hierarchical compliance operationdeterminer 114, which will be discussed later.

The desired floor reaction force distributor 102 groups (that is,hierarchizes) the first to the fourth ground contact portions 10 of therobot 1 into a tree structure, and associates the nodes of the treestructure with the hierarchized groups. Hence, in the followingexplanation, the nodes may be expressed by replacing them by the groups.Each node is a group constructed of one or more ground contact portions10.

In the first embodiment, the ground contact portions 10 arehierarchized, as shown in FIG. 6. More specifically, the n-th groundcontact portion 10 (n=1, 2, 3, 4) is associated with the n-th node, thenode having a first node and a fourth node as child nodes is defined asa 14th node, the node having a second node and a third node as childnodes is defined as a 23rd node, and the node having the 14th node andthe 23rd node as child nodes is defined as the 1423rd node. Thus, thefirst to the fourth nodes are nodes that are constructed of the first,the second, the third, and the fourth ground contact portions 10,respectively, the 14th node is a node constructed of the first groundcontact portion 10 and the fourth ground contact portion 10, the 23rdnode is a node constructed of the second ground contact portion 10 andthe third ground contact portion 10, and the 1423rd node is the nodeconstructed of all the ground contact portions 10.

According to general designations in a tree structure, a node having nochild node is referred to as a leaf node, and a node having no parentnode is referred to as a root node. Thus, the n-th node (n=1, 2, 3, 4)is a leaf node, while the 1423rd node is a root node. In the presentdescription, to identify nodes, leaf nodes will be assigned the samenumbers (1, 2, 3, . . . ) as those of the ground contact portions (orlegs) associated therewith, while nodes other than the leaf nodes willbe assigned with numbers that are greater than those of the leaf nodes.Further, the nodes other than the leaf nodes and the root node will bereferred to as intermediate nodes. In the first embodiment, theintermediate nodes are the 14th node and the 23rd node.

The desired floor reaction force distributor 102 corresponds to adesired node floor reaction force distributing means in the presentinvention, and the desired floor reaction force distributor 102 receivesa desired total floor reaction force central point trajectory, a desiredtotal floor reaction force trajectory and a desired ground contactportion trajectory out of a desired gait determined by the gaitgenerating device 100. The gait parameters (the estimated landingposition, an estimated landing time, etc. of the ground contact portion10 of a free leg of the robot 1) used by the gait generating device 100to determine the desired gait may be also input to the desired floorreaction force distributor 102. Then, on the basis of these inputs, thedesired floor reaction force distributor 102 determines desired nodefloor reaction force central points (desired n-th node floor reactionforce central points) Qn (n=1, 2, 3, 4, 14, 23, 1423), which are thedesired positions of the floor reaction force central points of thenodes hierarchized as described above, and weights Wn (n=1, 2, 3, 4, 14,23) of the nodes except the root node. Each weight Wn will take anonnegative value of 1 or less.

The technique for determining desired node floor reaction force centralpoints Qn (n=1, 2, 3, 4, 14, 23) and the weights Wn (n=1, 2, 3, 4, 14,23) will be explained in detail below with reference mainly to FIG. 3(b), FIG. 5 and FIG. 7. FIG. 3( b) shows a relationship between Qn and Wnin a state illustrated in the figure, FIGS. 5( a) to (c) show settingexamples of Wn, and FIG. 7 shows a relationship between weights anddesired node floor reaction force central points.

The desired node floor reaction force central point of the 1423rd node,which is the root node, (the desired 1423rd node floor reaction forcecentral point) Q1423 is determined to be identical to the desired totalfloor reaction force central point (point P in FIG. 3 and FIG. 4)(P=Q1423). Accordingly, in the following explanation, the same referencecharacter P as that of a desired total floor reaction force centralpoint will be frequently used as the mark denoting the desired 1423rdnode floor reaction force central point.

Further, a weight W1423 of the root node is set to “1” for the sake ofconvenience in order to maintain the uniformity of expressions.

In the present embodiment, the desired floor reaction force centralpoint of an n-th ground contact portion 10 (n=1, 2, 3, 4) (also referredto as a desired n-th ground contact portion floor reaction force centralpoint) is always set to the central point of an n-th ground contactportion 10 (the central point of the spherical joint 12). Further, thedesired node floor reaction force central point Qn (n=1, 2, 3, 4) of aleaf node associated with the n-th ground contact portion 10 isdetermined to be identical to the desired floor reaction force centralpoint of the n-th ground contact portion 10. Hence, the desired nodefloor reaction force central point Qn (n=1, 2, 3, 4) is also always setto the central point of the n-th ground contact portion 10 (the positionof this point is uniquely determined from a desired ground contactportion position of the n-th ground contact portion 10). The position ofthe desired node floor reaction force central point Qn (n=1, 2, 3, 4) ofthe leaf node determined as described above is uniquely determined fromthe desired ground contact portion position of the n-th ground contactportion 10. In other words, the desired node floor reaction forcecentral point Qn (n=1, 2, 3, 4) of a leaf node defines the desiredposition of the n-th ground contact portion 10. In the followingexplanation, the desired node floor reaction force central point of aleaf node may be referred to as a desired ground contact portion floorreaction force central point in some cases. Each desired ground contactportion trajectory is set such that it continuously changes, so that Qn(n=1, 2, 3, 4) will also continuously change.

Supplementally, in the present embodiment, the desired floor reactionforce central point Qn (n=1, 2, 3, 4) of an n-th leaf node (an n-thground contact portion 10) has been set to the central point of the n-thground contact portion 10; however, in a robot that allows the posturesof ground contact portions to be controlled, the desired floor reactionforce central point of each leaf node (each ground contact portion) maybe set, for example, within the ground contact surface of the groundcontact portion defined by the desired position/posture of the groundcontact portion associated with the leaf node (the surface that comes incontact with a floor surface assumed in a desired gait).

Hereinafter, in general, a segment that connects arbitrary two points Aand B or the length thereof will be denoted as AB. In addition, anoperator “*” means multiplication for a pair of scalar and scalar or apair of scalar and vector, while it means an outer product (vectorproduct) for a pair of vector and vector.

The weight Wn (n=1, 2, 3, 4, 14, 23) of each node except the root nodeand desired node floor reaction force central points Q14 and Q23 of theintermediate nodes are determined such that a desired 14th node floorreaction force central point Q14 will be the internally dividing pointof a segment Q1Q4 that satisfies a relational expression,Q1Q14:Q14Q4=W4:W1=(1−W1):W1, a desired 23rd node floor reaction forcecentral point Q23 will be the internally dividing point of a segmentQ2Q3 that satisfies a relational expression,Q2Q23:Q23Q3=W3:W2=(1−W2):W2, and a desired 1423rd node floor reactionforce central point P(=desired total floor reaction force central point)will be the internally dividing point of a segment Q14Q23 that satisfiesQ14P:PQ23=W23:W14=(1−W14):W14.

In other words, Q14, Q23 and Wn (n=1, 2, 3, 4, 14, 23) are determined tosatisfy at least the following expressions 1, 2 and 3. Incidentally, Qn(n=1, 2, 3, 4, 14, 23) in these expressions 1 to 3 means the position(positional vector) of the point.Q14=Q1*W1+Q4*W4, W1+W4=1  Expression 1Q23=Q2*W2+Q3*W3, W2+W3=1  Expression 2P=Q14*W14+Q23*W23, W14+W23=1  Expression 3

Wn (n=1, 2, 3, 4, 14, 23) takes a nonnegative value of 1 or less, sothat the coefficients (weights) of Qn (n=1, 2, 3, 4, 14, 23) in theright sides of the above expressions 1, 2 and 3 will be all nonnegativevalues.

The above expressions 1 to 3 mean that the position of a desired nodefloor reaction force central point of each node having child nodes (thatis, each node that is not a leaf node) is set to a weighted averageposition of the positions of the desired node floor reaction forcecentral points of the child nodes of the node by using a predeterminednonnegative weight. In other words, as shown in FIG. 3( b) and FIG. 7mentioned above, the desired node floor reaction force central point Qn(n=14, 23, 1423) of each node having child nodes is set at theinternally dividing point of the desired node floor reaction forcecentral points of all the child nodes of the node. FIG. 7 is a diagramshowing a relationship between the desired node floor reaction forcecentral points Qn (n=1, 2, 3, 4, 14, 23, 1423) of nodes and the weightsWn (n=1, 2, 3, 4, 14, 23). Incidentally, L23, L14 and L1423 in FIG. 3(b) denote segments Q2Q3, Q1Q4 and Q23Q14, respectively.

Supplementally, Q1 to Q4 and P(=Q1423) are determined as describedabove, so that once Wn (n=1, 2, 3, 4, 14, 23) is determined, Q14 and Q23that satisfy expressions 1 to 3 are uniquely determined. In other words,if Q14 and Q23 are determined, then Wn (n=1, 2, 3, 4, 14, 23) satisfyingexpressions 1 to 3 will be uniquely determined. Accordingly, the weightsWn (n=1, 2, 3, 4, 14, 23) may be determined and then the desired nodefloor reaction force central points Q14 and Q23 of intermediate nodesaccording to the above expressions 1 to 3 may be determined, or thedesired node floor reaction force central points Q14 and Q23 of theintermediate nodes may be determined and then the weights Wn (n=1, 2, 3,4, 14, 23) may be determined according to the above expression 1 to 3.Either way may be used.

The desired node floor reaction force central points Q14 and Q23 of theintermediate nodes move as the ground contact portions 10 move, as shownin time series in FIG. 3( a) to FIG. 3( c) and FIG. 4( a) to FIG. 4( c).Weights W14(=1−W23), W1(=1−W4) and W3(=1−W2) at this time are determinedsuch that they continuously change, as shown in, for example, the graphsof FIGS. 5( a) to (c), respectively. The trajectory of Qn and the weightWn of the n-th node (n=1, 2, 3, 4) are set such that they continuouslychange, so that the desired node floor reaction force central points Q14and Q23 are also set such that they continuously move. This means thatall desired node floor reaction force central points (desired n-th nodefloor reaction force central points (n=1, 2, 3, 4, 14, 23, 1423)) areset such that they continuously move.

The desired floor reaction force distributor 102 outputs the desirednode floor reaction force central point of each node determined asdescribed above. The desired floor reaction force central point of theroot node is the same as the desired total floor reaction force centralpoint determined by the gait generating device 100, so that it does nothave to be output from the desired floor reaction force distributor 102.

The desired floor reaction force distributor 102 determines and outputsa desired node floor reaction force, which is the desired value of thefloor reaction force acting on the desired floor reaction force centralpoint of each node. The desired node floor reaction force that is outputincludes at least a desired node floor reaction force (a desired n-thnode floor reaction force) acting on a desired node floor reaction forcecentral point Qn of an n-th node (n=1, 2, 3, 4), which is a leaf node,that is, the desired floor reaction force of each ground contact portion10. This desired node floor reaction force is necessary primarily tocompensate for the flexures of the compliance mechanisms 42 or the like(refer to FIG. 1) of the legs #1 to #4 (more specifically, compensationfor the positional displacement of the ground contact portions 10 causedby the flexures of the compliance mechanisms 42 and the link mechanismsof the legs), the compensation being carried out by the processing of ahierarchical compliance operation determiner 114, which will bediscussed later. Supplementally, if the robot 1 is provided withcompliance mechanisms in addition to those on the distal portions of thelegs #1 to #4, then preferably, a desired 14th node floor reaction forceand a desired 23rd node floor reaction force (the desired node floorreaction forces of intermediate nodes) are also determined and output inorder to determine the deformations of the compliance mechanisms.

Generally, a desired floor reaction force (a desired node floor reactionforce) acting on a desired node floor reaction force central point ofeach node may be determined from a desired total floor reaction forceand the weight of each node. Specifically, the desired floor reactionforce of any one node may be determined by multiplying the product ofthe weight of the node and the weight of all ancestor nodes of the nodeby a desired total floor reaction force. More specifically, a desiredn-th node floor reaction force is calculated according to the followingexpression 4a (or expression 4b).Desired n-th node floor reaction force=Weight of the n-th node*Productof the weights of all ancestor nodes of the n-th node*Desired totalfloor reaction force  Expression 4aDesired n-th node floor reaction force=Weight of the n-th node*Productof the weight of all ancestor nodes (excluding the root node) of then-th node*Desired total floor reaction force  Expression 4b

Alternatively, desired node floor reaction forces are determined suchthat the desired floor reaction force of an arbitrary n-th node that isnot a leaf node agrees with the sum (resultant force) of the desiredfloor reaction forces of all child nodes of the n-th node and thedesired floor reaction force of the root node agrees with a desiredtotal floor reaction force. This relationship is shown in FIG. 8.

More specifically, if a desired n-th node floor reaction force, which isthe desired floor reaction force of an n-th node, is denoted by Fn (n=1,2, 3, 4, 14, 23, 1423) and a desired total floor reaction force isdenoted by Ftotalref, then Fn is determined from Ftotalref and theweight Wn (n=1, 2, 3, 4, 14, 23) according to the expressions shown inFIG. 8. The expressions in FIG. 8 are equivalent to the above expression4a or 4b.

Determining the desired floor reaction force central point of each node(a desired node floor reaction force central point) and the desiredfloor reaction force of each node (a desired node floor reaction force)as described above is to determine the desired floor reaction forcecentral point and the desired floor reaction force of each node suchthat the horizontal component of the moment generated about a desiredn-th node floor reaction force central point by the resultant force ofthe desired floor reaction forces of all child nodes of an n-th nodebecomes zero. Accordingly, the moment horizontal component of a desirednode floor reaction force is zero for any nodes.

A set of each desired node floor reaction force central point Qn (n=1,2, 3, 4, 14, 23), each weight Wn (n=1, 2, 3, 4, 14, 23) and each desirednode floor reaction force Fn (n=1, 2, 3, 4, 14, 23) is determined suchthat the desired floor reaction force of the ground contact portion 10not in contact with the ground (in a no-contact-with-ground state)(thedesired node floor reaction force of a leaf node associated with theground contact portion 10 not in contact with the ground) is zero.Hence, in the first embodiment, the weight W14 is set to zero (W23=1)during a period in which the first ground contact portion 10 and thefourth ground contact portion 10 are free legs and off a floor (theground contact portions 10 of the free legs are moving in the air),while the weight W23 is set to zero (W14=1) during a period in which thesecond ground contact portion 10 and the third ground contact portion 10are free legs and off a floor, as shown in FIG. 5( a). Thus, the weightof an intermediate node having child nodes is set to zero during aperiod in which none of the ground contact portions belonging as leafnodes to the intermediate node are in contact with the ground. In otherwords, the weight of an intermediate node is not set to zero during aperiod in which any one of the ground contact portions belonging to theintermediate node is in contact with the ground (strictly speaking, aperiod during which a non-zero floor reaction force is acting on any oneof the ground contact portions).

Supplementally, the first embodiment does not have a period during whichonly one of the first ground contact portion 10 and the fourth groundcontact portion 10 belonging to the intermediate node Q14 is placed in ano-contact-with-ground state; therefore, the weights W1 and W4corresponding to these ground contact portions 10 and 10 do not have anyperiods during which they are 0 or 1. If, however, a desired groundcontact portion trajectory is determined so as to include a periodduring which only one of the first ground contact portion 10 and thefourth ground contact portion 10 will be placed in theno-contact-with-ground state, then the weight associated with the groundcontact portion 10 that will be placed in the no-contact-with-groundstate during the period may be set to 0, and the weight associated withthe ground contact portion 10 to be in contact with the ground may beset to 1. In this case, the weight of the intermediate node Q14 duringthat period will be set to a non-zero value. The same applies to theweights associated with the intermediate node Q23 and the second groundcontact portion 10 and the third ground contact portion 10, which areleaf nodes belonging thereto. Generally speaking, the weight of a nodehaving child nodes is set to a non-zero value if any one of the groundcontact portions belonging to the node is in contact with the ground,while it is set to zero if all of the ground contact portions belongingto the node are in the no-contact-with-ground state.

The desired node floor reaction forces determined as described abovecontinuously change, making them suited for achieving a movement(walking) of the robot 1 with less impact.

As described above, according to the first embodiment, each desired nodefloor reaction force central point Qn (n=1, 2, 3, 4, 14, 23, 1423), eachweight Wn(n=1, 2, 3, 4, 14, 23) and each desired node floor reactionforce Fn (n=1, 2, 3, 4, 14, 23, 1423) are determined by the desiredfloor reaction force distributor 102 such that they satisfy thefollowing conditions A) to G).

-   A) The desired node floor reaction force central point Qn (n=1, 2,    3, 4) of each leaf node agrees with the central point of the ground    contact portion 10 corresponding to the leaf node. More generally,    Qn (n=1, 2, 3, 4) is determined according to a desired gait (a    desired motion, such as a desired ground contact portion    trajectory). Supplementally, for example, the desired floor reaction    force central point of each ground contact portion 10 may be    determined by the gait generating device 100, and in this case, the    desired node floor reaction force central point Qn (n=1, 2, 3, 4)    may be determined on the basis of the desired floor reaction force    determined by the gait generating device 100.-   B) The desired node floor reaction force central point of the root    node agrees with a desired total floor reaction force central point    P.-   C) The desired node floor reaction force central point Qn (n=1, 2,    3, 4, 14, 23) and the weight Wn (n=1, 2, 3, 4, 14, 23) of each node    excluding the root node satisfy the relational expressions of the    above expressions 1 to 3. In other words, the desired node floor    reaction force central point Qn of an arbitrary n-th node (n=14,    23, 1423) having child nodes will be a weighted average point of the    desired node floor reaction force central points of the child nodes    of the n-th node.-   D) The desired node floor reaction force Fn of an arbitrary n-th    node (n=14, 23, 1423) having child nodes agrees with the sum    (resultant force) of the desired node floor reaction forces of all    child nodes of the n-th node, and the desired node floor reaction    force F1423 of the root node (the 1423rd node) agrees with the    desired total floor reaction force Ftotalref. In other words, the    desired floor reaction force Fn and the weight Wn of the n-th node    (n=1, 2, 3, 4, 14, 23, 1423) satisfy the relational expressions in    FIG. 8.-   E) The desired node floor reaction force of a leaf node associated    with the ground contact portion 10 not in contact with the ground is    zero.-   F) Qn, Wn, and Fn (n=1, 2, 3, 4, 14, 23, 1423) continuously change.-   G) The weight of a leaf node corresponding to the ground contact    portion 10 in a no-contact-with-ground state or the weight of any    one of the ancestor nodes of the leaf node is zero.

Incidentally, a desired node floor reaction force may be determined onthe basis of each desired node floor reaction force central point inplace of determining it on the basis of a weight as described above.More specifically, each desired node floor reaction force central pointmay be determined such that the aforesaid conditions A) to C) aresatisfied, then each weight may be determined on the basis of thedesired node floor reaction force central point and the aforesaidexpressions 1 to 3, and then the desired node floor reaction force maybe determined according to the aforesaid expression 4 by using thedetermined weight.

Returning to the explanation of FIG. 2, the posture error calculator 103calculates the error of an actual body posture with respect to a desiredbody posture and outputs the calculated error to the robot 1. In thepresent embodiment, the posture error calculator 103 receives aninclination angle of the body 24 relative to the vertical directiondetected by the posture sensor 36 (hereinafter referred to as the actualbody posture inclination) and a desired body position/posture determinedby the gait generating device 100 (specifically, the inclination anglein a desired body posture relative to the vertical direction;hereinafter referred to as the desired body posture inclination), andthen calculates an error θberr between them (Actual body postureinclination——Desired body posture inclination; hereinafter referred toas the body posture inclination error θberr). The calculated θberr iscomposed of a component about the X axis (the component in the rolldirection) θberrx and a component about the Y axis (the component in thepitch direction) θberry. If a desired body posture inclination is, forexample, steadily zero, then θberr=actual body posture inclination, sothat a detected value (actual body posture inclination) of a posturesensor 36 may be directly output as the body posture inclination errorθberr.

Generally speaking, the posture stabilization control calculator 104calculates a compensating total floor reaction force, which is acompensation amount of a total floor reaction force (the correctionamount of a desired total floor reaction force) for stabilizing theposture of the robot 1 according to the state of the robot 1 that isdetected or estimated on the basis of the information of a sensorprovided in the robot 1, such as the aforesaid body posture inclinationerror.

To stabilize the posture of the robot 1 in the long term, atranslational force and a moment necessary to restore the actualposition/posture of a predetermined portion, such as the body 24, of therobot 1 to a desired position/posture are determined, and this has to beadditionally generated, the desired total floor reaction force centralpoint (desired ZMP) being the point of action. The additionaltranslational force and moment are referred to as a compensating totalfloor reaction force. The moment component of a compensating total floorreaction force is referred to as a “compensating total floor reactionforce moment Mdmd” (specifically, a compensating total floor reactionforce moment Mdmd about a desired total floor reaction force centralpoint (desired ZMP)).

In the present embodiment, the posture stabilization control calculator104 calculates the compensating total floor reaction force moment Mdmdso as to restore (approximate) an actual body posture inclination to adesired body posture inclination. Hence, the body posture inclinationerrors θberr (θberrx, θberry) determined by the posture error calculator103 are input to the posture stabilization control calculator 104.

The posture stabilization control calculator 104 calculates thecompensating total floor reaction force moment Mdmd on the basis of theinput body posture inclination error θberr. The calculated Mdmd iscomposed of a component about the X axis Mdmdx and a component about theY axis Mdmdy.

Specifically, Mdmdx and Mdmdy are determined by, for example, thefeedback control law (PD control law here) of expressions 5 and 6 givenbelow. More specifically, Mdmdx and Mdmdy are determined so that thebody posture inclination errors θberrx and θberry approximate zero.Mdmdx=−Kthx*θberrx−Kwx*(dθberrx/dt)  Expression 5Mdmdy=−Kthy*θberry−Kwy*(dθberry/dt)  Expression 6where Kthx, Kthy, Kwx and Kwy denote predetermined gains. Further,(dθberrx/dt) and (dθberry/dt) denote the time differential values of thebody posture inclination errors θberrx and θberry, respectively.

In the present embodiment, a component of the compensating total floorreaction force moment Mdmd about the Z axis (the component in the yawdirection) Mdmdz is not used, so that Mdmdz is not determined; however,Mdmdz may be determined to prevent the robot 1 from spinning (slippageabout the vertical axis). A method for determining Mdmdz is explained indetail in Japanese Patent Application No. 2003-185613 or Japanese PatentApplication No. 2003-185930 previously proposed by the presentapplicant. Moreover, for example, to restore the position of an actualcenter-of-gravity of the robot 1 to the position of thecenter-of-gravity in a desired gait, the translational force of acompensating total floor reaction force can be determined on the basisof a positional error of the center-of-gravity.

The floor reaction force detector 108 detects the actual floor reactionforces, which are the actual values of the floor reaction forces actingon the ground contact portions 10 of the actual robot 1 (that is, theactual floor reaction forces of leaf nodes (actual node floor reactionforces)) on the basis of the outputs of the six-axis force sensor 34 ofthe legs #1 to #4. In addition, the floor reaction force detector 108calculates the relative positions/postures (the relative positions inthe first embodiment) of the ground contact portions 10 relative to thecoordinate system fixed to the body 24 on the basis of the actual jointdisplacements of the joints 14 and 15 (the actual rotational angles ofthe joints 14 and 15 about individual rotational axes) of the legs #1 to#4 detected by sensors, such as encoders, (not shown) provided on thejoints 14 and 15 of the robot 1. At this time, joint displacementcommands, which are the displacement command values (rotational anglecommand values) of the joints 14 and 15, may be used in place of actualjoint displacements, or both actual joint displacements and jointdisplacement commands may be used. Then, based on the calculatedrelative positions/postures of the ground contact portions 10, thedetected values of the six-axis force sensor 34 of the legs #1 to #4(these being the values on a local coordinate system fixed to thesix-axis force sensor 34 or the like) are coordinate-converted tocalculate actual floor reaction forces represented on the coordinatesystem fixed to the body 24, and then the calculated actual floorreaction forces are converted into the actual floor reaction forcesrepresented on a supporting leg coordinate system (global coordinatesystem). For the coordinate conversion into the supporting legcoordinate system, detected values of the posture sensor 36 or desiredbody posture inclinations may be used. Supplementally, in the robot 1according to the first embodiment, no floor reaction force moment actson the central points of the ground contact portions 10, as describedabove, so that there is no need to detect a moment component in theactual floor reaction force of each ground contact portion 10. In thiscase, as mentioned above, in place of the six-axis force sensor 34, athree-axis force sensor may be used to detect translational forcecomponents of actual floor reaction forces in three axes, or a one-axisfloor reaction force sensor may be used to detect only translationalforce vertical components of actual floor reaction forces.

Based on a final desired trajectory of each ground contact portionposition/posture (this being determined by a hierarchical complianceoperation determiner 114, which will be discussed later) and desiredbody position/posture or the like, the robot geometric model (inversekinematics calculator) 110 performs inverse kinematics calculation tocalculate joint displacement commands, which are the command values ofthe displacements (rotational angles) of the joints 14 and 15 of therobot 1, that satisfy the above. In the present embodiment, the equationof the solution of the inverse kinematics calculation has beendetermined beforehand, and the joint displacement commands have beencalculated simply by substituting desired body position/posture and thefinal desired position of each ground contact portion into the equation.More specifically, the robot geometric model 110 receives a desired bodyposition/posture trajectory determined by the gait generating device 100and a corrected desired ground contact portion trajectory corrected aswill be discussed later by the hierarchical compliance operationdeterminer 114 (a corrected desired ground contact portion trajectorywith deformation compensation), and calculates the joint displacementcommands of the joints 14 and 15 of the legs #1 to #4 by the inversekinematics calculation from the received values.

In the case of a robot having joints, such as arm joints and a neckjoint, in addition to leg joints, as in the sixth embodiment to bediscussed later, the displacements of joints other than the leg jointsare determined by the inverse kinematics calculation on the basis of therelative positions/postures of hands, the head or the like with respectto the body.

The displacement controller 112 receives the actual joint displacements(detected values) of the joints 14 and 15 of the robot 1 and the jointdisplacement commands calculated by the robot geometric model (inversekinematics calculator) 110, and controls (feedback-control) actuators(not shown) of the joints 14 and 15 by using the joint displacementcommands as the desired values such that the actual joint displacementsfollow the desired values.

The hierarchical compliance operation determiner 114 corrects a desiredground contact portion trajectory such that an actual total floorreaction force approximates the resultant force of a desired total floorreaction force and a compensating total floor reaction force, andoutputs a corrected desired ground contact portion position/posturetrajectory with deformation compensation, which is the desired groundcontact portion trajectory after the correction. In the presentembodiment, the postures of the ground contact portions 10 cannot becontrolled; therefore, the corrected desired ground contact portionposition/posture trajectory with deformation compensation is actually acorrected desired ground contact portion position trajectory withdeformation compensation.

The hierarchical compliance operation determiner 114 generally correctsthe desired ground contact portion trajectories of the ground contactportions 10 so as to satisfy the following three requirements as much aspossible.

-   Requirement 1) In order to stabilize the position/posture of the    robot 1, an actual total floor reaction force is made to follow the    resultant force of a compensating total floor reaction force (moment    Mdmd) output by the posture stabilization control calculator 104 and    a desired total floor reaction force. According to the first    embodiment, in order to stabilize the posture inclination (the    inclination relative to the vertical direction) of the body 24 of    the robot 1, the horizontal component of an actual total floor    reaction force moment about a desired total floor reaction force    central point is made to follow horizontal components Mdmdx and    Mdmdy of the compensating total floor reaction force moment Mdmd.    Supplementally, the horizontal component of the desired total floor    reaction force moment about the desired total floor reaction force    central point is zero, so that the resultant force of this and Mdmd    agrees with Mdmd.-   Requirement 2) The absolute value of the actual floor reaction force    moment about a desired floor reaction force central point of each    node, which is not a leaf node, is minimized as much as possible to    prevent an actual floor reaction force from being focused on some    ground contact portions 10 of a plurality of ground contact portions    10 to be in contact with the ground, causing the actual floor    reaction force on other ground contact portions 10 to be excessively    reduced, which will lead to extremely deteriorated ground contacting    properties of the ground contact portions 10 having the reduced    actual floor reaction force. In the first embodiment, the absolute    values of the actual floor reaction force moments about the desired    floor reaction force central points of the 14th node, the 23rd node    and the 1423rd node are minimized as much as possible.-   Requirement 3) In order to secure the ground contacting properties    of the ground contact portions 10 to be in contact with the ground,    that is, to prevent local ground contacting properties of the ground    contact portions 10 from deteriorating due to uneven local ground    contact pressure distribution (the distribution of an actual floor    reaction force) in the ground contact portions 10, the absolute    values of the actual floor reaction force moments about the desired    floor reaction force central points of the ground contact portions    10 (leaf nodes) are minimized as much as possible. However, in the    robot 1 of the first embodiment, the actual floor reaction force    moments about the desired floor reaction force central points of the    ground contact portions 10 are always zero; therefore, it is    unnecessary to consider this requirement 3).

In general, it is impossible to fully satisfy all of requirements 1) to3) or requirements 1) and 2). For example, it is often physicallyimpossible to zero the actual floor reaction force moments about thedesired floor reaction force central points of the ground contactportions 10 while making an actual total floor reaction force agree withthe resultant force of a compensating total floor reaction force and adesired total floor reaction force. Hence, the hierarchical complianceoperation determiner 114 usually corrects the desired ground contactportion trajectories of the ground contact portions 10 at certaincompromise points while satisfying requirements 1) to 3) orrequirements 1) and 2) as much as possible.

The above has given the overviews of the functional means (functionalcomponents) of the control device 50.

Supplementally, the hierarchical compliance operation determiner 114,the posture stabilization control calculator 104 and a robot geometricmodel (inverse kinematics calculator) 110 correspond to the nodeoperation control means in the present invention.

Referring now to the flowchart of FIG. 9, the overall operation(arithmetic processing) of the control device 50 will be explained inmore detail. FIG. 9 is a flowchart (structured flowchart) showing themain routine processing of the control device 50. The components of thecontrol device 50 carrying out pertinent processing are shown on theleft in FIG. 9.

First, the initialization of the control device 50 is performed in S10,then the processing advances to S14 via S12. The arithmetic processingof the control device 50 waits for a timer interrupt for each controlcycle. The control cycle is, for example, 50 ms.

Subsequently, the processing proceeds to S16 wherein it determineswhether a gait change is observed, and if the determination result isNO, then it proceeds to S22, which will be discussed later. If thedetermination result in S16 is YES, then the processing proceeds to S18wherein it initializes time t to zero, and proceeds to S20 wherein itsets gait parameters. In the present embodiment, for example, a desiredgait of a predetermined period from the moment a predetermined leg(e.g., #1) of the robot 1 leaves a floor to the moment it leaves thefloor next (or from the moment it lands to the moment it lands next) istaken as a unit, and gait parameters that are the parameters definingthe desired gait for the predetermined period (the parameters used inthe algorithm for determining the desired gait) are set in S20. The“gait change” mentioned in S16 means the change of the desired gait forthe predetermined period. Whether the desired gait is changing may bedetermined mainly on the basis of time or a detected value of thesix-axis force sensor 34 on the predetermined leg.

The gait parameters set in S20 are composed of motion parameters thatdefine the desired motion trajectories (specifically, a desired groundcontact portion trajectory and a desired body position/posturetrajectory) of the robot 1 and the floor reaction force parameters thatdefine desired floor reaction force trajectories (specifically, adesired total floor reaction force trajectory and a desired total floorreaction force central point trajectory). Supplementally, once thedesired motion of the robot 1 is determined, the desired total floorreaction force can be obtained by reversing the sign of the resultantforce of the inertial force generated by the desired motion and thegravity acting on the robot 1, so that the floor reaction forceparameters may be the ones that define only a desired total floorreaction force central point trajectory. Further, motion parameters donot have to include parameters that define a desired bodyposition/posture trajectory if a desired ground contact portiontrajectory (more generally, the parameter of a desired motion other thandesired body position/posture) and a desired total floor reaction forcecentral point are determined, and then desired body position/posture aredetermined using a dynamic model of the robot 1 such that the horizontalcomponent of a moment generated about a desired total floor reactionforce central point (desired ZMP) by the resultant force of the inertialforce generated by a desired motion, including desired bodyposition/posture of the robot 1, and the gravity acting on the robot 1becomes zero.

Subsequently, the processing proceeds to S22 wherein the instantaneousvalue of a desired gait is determined on the basis of the aforesaid gaitparameters. Here, “instantaneous value” means a value for each controlcycle, and a desired gait instantaneous value is composed of theinstantaneous values of desired body position/posture, a desired groundcontact portion position (the instantaneous value of a desired groundcontact portion trajectory), a desired total floor reaction force, and adesired total floor reaction force central point position (a desired ZMPposition). In the present embodiment, the postures of the ground contactportions 10 cannot be controlled, so that the instantaneous values ofthe desired postures of the ground contact portions 10 are notdetermined. In a case where the postures of the ground contact portionscan be controlled, the parameters defining the desired postures of theground contact portions may be included in the gait parameters and theinstantaneous values of the desired postures of the ground contactportions may be determined on the basis of the parameters.

The processing of S14 to S22 described above is the processing carriedout by the gait generating device 100.

Subsequently, the processing proceeds to S24 wherein the weight Wn (n=1,2, 3, 4, 14, 23) of each node and a desired floor reaction force centralpoint of each node (a desired node floor reaction force central point)Qn (n=1, 2, 3, 4, 14, 23) are determined. This processing is theprocessing carried out as described above by the desired floor reactionforce distributor 102.

Subsequently, the processing proceeds to S26 wherein desired node floorreaction forces (including at least the desired floor reaction forces ofthe ground contact portions 10 (leaf nodes)) are determined. In thefirst embodiment, the desired node floor reaction forces of leaf nodes(desired ground contact portion floor reaction forces) are determined.This processing of S26 is also the processing carried out as describedabove by the desired floor reaction force distributor 102. As describedabove, if the compliance mechanisms are provided on other portions inaddition to the distal portions of the legs #1 to #4, then the desiredfloor reaction forces of intermediate nodes that are not leaf nodesshould be also determined. Supplementally, the moment horizontalcomponent of a desired node floor reaction force is zero.

Subsequently, the processing proceeds to S28 wherein the state of therobot 1, such as the actual body posture inclination, is detected froman output of the posture sensor 36 or the like. In the first embodiment,the value of an actual body posture inclination detected by the posturesensor 36 is captured by the posture error calculator 103, and a bodyposture inclination error θberr is calculated from the detected valueand the desired body posture inclination (the instantaneous value at thecurrent time) out of the desired body position/posture.

Subsequently, the processing proceeds to S30 wherein a compensatingtotal floor reaction force for stabilizing the posture of the robot 1 isdetermined from the state of the robot 1 detected in S28. In the firstembodiment, the horizontal components Mdmdx and Mdmdy of a compensatingtotal floor reaction force moment Mdmd about a desired total floorreaction force central point (desired ZMP) are calculated by the posturestabilization control calculator 104 from the body posture inclinationerror θberr according to the above expression 5 and expression 6.

Subsequently, the processing proceeds to S32 wherein the actual floorreaction force of each ground contact portion 10 is detected. This isthe processing carried out by the actual floor reaction force detector108. As described above, the actual floor reaction force for each groundcontact portion 10 detected by the six-axis force sensor 34 that isconverted to a supporting leg coordinate system (global coordinatesystem) is determined. Hereinafter, the actual floor reaction force ofeach ground contact portion 10 may be referred to as an actual groundcontact portion floor reaction force in some cases.

Subsequently, from S34 to S38, the processing of the hierarchicalcompliance operation determiner 114 is carried out.

For a while, the following will explain the overall processing of thehierarchical compliance operation determiner 114 before the processingof these S34 to S38 is specifically explained. In this explanation, forthe convenience of understanding, the state shown in FIG. 3( b)mentioned above (the state wherein all the legs #1 to #4 of the robot 1are supporting legs) will be primarily taken as an example.

The hierarchical compliance operation determiner 114 determines thetranslational force components and moment components of the desired nodefloor reaction forces of nodes excluding leaf nodes (more specifically,the translational force components and the moment components that havedesired node floor reaction force central points as the points ofaction) mainly on the basis of the desired node floor reaction forces ofthe leaf nodes determined by the desired floor reaction forcedistributor 102.

The translational force components of the desired floor reaction forces(desired node floor reaction forces) of the nodes in the state shown inFIG. 3( b) are illustratively shown in FIG. 10. In this figure, a vectorFn_ref (n=1, 2, 3, 4, 14, 23) denotes the translational force componentof a desired n-th node floor reaction force. Further, Ftotalref denotesthe translational force component of a desired total floor reactionforce(=desired 1423rd node floor reaction force). As shown in FIG. 8mentioned above, a desired n-th node floor reaction force of anarbitrary n-th node (n=14, 23, 1423), which is not a leaf node, isdetermined by the resultant force of the desired node floor reactionforces of all child nodes of the n-th node. Therefore,F14ref=F1ref+F4ref, F23ref=F2ref+F3ref, andFtotalref(=F1423ref)=F14ref+F23ref. The hierarchical complianceoperation determiner 114 determines the translational force componentsF14ref(=F1ref+F4ref) and F23ref(=F2ref+F3ref) of the desired node floorreaction forces of intermediate nodes, as described above, from thetranslational force components Fn_ref (n=1, 2, 3, 4) of the desired nodefloor reaction forces of the leaf nodes (the ground contact portion 10)determined by the desired floor reaction force distributor 102.Ftotalref(=F1423ref) is set to the translational force component of thedesired total floor reaction force determined by the gait generatingdevice 100.

Furthermore, the hierarchical compliance operation determiner 114determines the moment component of a desired node floor reaction forceof each node, excluding leaf nodes, a desired node floor reaction forcecentral point being the point of action, as in the case of thetranslational force component of a desired node floor reaction force. Inthis case, however, according to the definition of a desired node floorreaction force central point Qn (n=1, 2, 3, 4, 14, 23), the momenthorizontal component of a desired n-th node floor reaction force isalways set to zero. The moment horizontal component of a desired 1423rdnode floor reaction force(=desired total floor reaction force) is alsoset to zero.

In the robot 1 of the present embodiment, the ground contact portions 10are engaged with the spherical joints 12 (free joints) at the distal endportions of the legs #1 to #4, so that floor reaction force moments(horizontal components and vertical components) cannot be generated inthe ground contact portions 10 (leaf nodes). For this reason, in thehierarchical compliance operation determiner 114, the moment verticalcomponents of the desired node floor reaction forces of the groundcontact portions 10 (leaf nodes) are also set to zero.

If a desired node floor reaction force moment vertical component of anode that is not a leaf node is determined so as to dynamically balancewith a desired motion of the robot 1, then it could generally take anon-zero value; however, in the present embodiment, the control relatedto the rotation (the rotation in the yaw direction) of the posture ofthe robot 1 about the vertical axis is not carried out. In the presentembodiment, therefore, the setting of the vertical component of themoment of a desired node floor reaction force of a node that is not aleaf node will be omitted. For this reason, the moment components ofdesired node floor reaction forces in the state shown in FIG. 3( b) willnot be shown. If the control related to the rotation of the posture ofthe robot 1 about the vertical axis is carried out, then the desiredfloor reaction force moment vertical components of the nodes should bealso set.

Supplementally, if the translational force components and the momentcomponents of the desired node floor reaction forces of the nodes,including leaf nodes, are determined by the desired floor reaction forcedistributor 102, then the determination need not be performed by thehierarchical compliance operation determiner 114.

Further, the hierarchical compliance operation determiner 114 alsodetermines the translational force component and the moment component ofan actual node floor reaction force, which is the actual floor reactionforce of each node.

The translational force components of the actual floor reaction forcesof the nodes (actual node floor reaction forces) in the state shown inFIG. 3( b) are illustratively shown in FIG. 11. In the diagram, a vectorFn_act (n=1, 2, 3, 4, 14, 23) denotes a translational force component ofan actual n-th node floor reaction force. Further, Ftotalact denotes thetranslational force component of an actual total floor reactionforce(=actual 1423rd node floor reaction force). In general, thetranslational force component of the actual floor reaction force of eachnode that is not a leaf node is the resultant force of the translationalforce components of the actual floor reaction forces of all child nodesof the node. Accordingly, the translational force components of theactual floor reaction forces of the 14th node, the 23rd node, and the1423rd node are F14act=F1act+F4act, F23act=F2act+F3act,Ftotalact(=F1423act)=F14act+F23act. The translational force componentsF1act, F2act, F3act and F4act of the actual floor reaction forces of theleaf nodes are the translational force components of the actual floorreaction forces (actual ground contact portion floor reaction forces) ofthe ground contact portions 10 obtained by the actual floor reactionforce detector 108. The vectors shown by dashed lines in FIG. 11indicate the translational force components of the desired node floorreaction forces shown in FIG. 10 mentioned above. The hierarchicalcompliance operation determiner 114 determines the translational forcecomponents of the actual node floor reaction forces of the nodes fromthe actual floor reaction forces of the ground contact portions 10obtained by the actual floor reaction force detector 108.

The moment components of the actual floor reaction forces of the nodesin the state shown in FIG. 3( b) are illustratively shown in FIG. 12. Inthe diagram, a vector Mn_act (n=14, 23, 1423) denotes the momentcomponent of an actual n-th node floor reaction force. In this case, aswith the translational force components of the actual floor reactionforces of the nodes, in general, the moment component of an actual floorreaction force M14act, M23act or M1423act(=Mtotalact) of a node that isnot a leaf node is defined as the moment component of the resultantforce of the actual floor reaction forces of all child nodes of the node(the moment component having the desired floor reaction force centralpoint Q14, Q23 or Q1423 of the node as the point of action).

In the robot 1 of the present embodiment, no floor reaction force momentcan be generated in the ground contact portions 10 (leaf nodes), asdescribed above, so that the moment components of the actual floorreaction forces of leaf nodes (actual ground contact portion floorreaction forces) will be always zero. Thus, the moment components of theactual node floor reaction forces of the leaf nodes will not be shown.

The actual floor reaction force moment components of nodes that are notleaf nodes (in the present embodiment, M14act, M23act and M1423act) arenot generally zero. For example, a moment is usually generated about thedesired 14th node floor reaction force central point Q14 by thehorizontal components of the translational force components of theactual floor reaction forces of the first ground contact portion 10 (the1st node) and the fourth ground contact portion 10 (the 4th node).However, in the robot 1 in the present embodiment, the distal endportions of the legs are provided with the spherical joints 12, whichare free joints, so that a component in the same direction as a segmentQ1Q4 of M14act and a component in the same direction as a segment Q2Q3of M23act will be zero.

In a robot having the distal joints of its legs provided with actuators(a robot in which the postures of the distal ground contact portions ofthe legs are controllable), even if the control related to the rotationof the posture of the robot about the vertical axis is not carried out,the desired floor reaction force moment horizontal components of theground contact portions are also set and the actual floor reaction forcemoments of the ground contact portions are also detected, as shown in anembodiment disclosed in Japanese Unexamined Patent ApplicationPublication No. H10-277969 previously proposed by the present applicant.Then, an operation for correcting the postures of the ground contactportions should be performed so that the actual floor reaction forcemoment horizontal component of each ground contact portion approximatesa desired floor reaction force moment horizontal component or that theactual floor reaction force moment of each ground contact portionapproximates the sum (the vector sum) of the desired floor reactionforce moment horizontal component and its ground contact portioncompensating floor reaction force moment.

The processing of the hierarchical compliance operation determiner 114will be explained in more detail. In this case, a situation is assumedin which the posture of the body 24 of the robot 1 is about to falltoward left rear in the state shown in FIG. 3( b) and the compensatingtotal floor reaction force moment Mdmd determined by the posturestabilization control calculator 104 is as shown in FIG. 13.

To restore the posture (the inclination relative to the verticaldirection) of the body 24 of the robot 1 (to restore the inclination ina desired body posture), the horizontal component of an actual totalfloor reaction force moment about a desired total floor reaction forcecentral point (a desired ZMP) may be made to follow the horizontalcomponent of the sum of a desired total floor reaction force momentMtotalref(=M1423ref) and a compensating total floor reaction forcemoment Mdmd.

Meanwhile, at the desired total floor reaction force central point (thedesired ZMP), the horizontal component of the desired total floorreaction force moment Mtotalref is zero. Hence, to restore the posture(inclination) of the body 24 of the robot 1 in the longitudinal andlateral directions, the horizontal component of the actual total floorreaction force moment about the desired total floor reaction forcecentral point (the desired ZMP) may be made to follow the horizontalcomponents (Mdmdx, Mdmdy) of Mdmd. Further, in the present embodiment,the actual floor reaction force moment about the desired floor reactionforce central point of each ground contact portion 10 is zero.

Therefore, the hierarchical compliance operation determiner 114 in thefirst embodiment corrects the desired ground contact portion position(especially the position in the height direction) of each ground contactportion 10 determined by the gait generating device 100 so as to satisfythe aforesaid requirements 1) and 2) as much as possible.

To make the correction, the hierarchical compliance operation determiner114 determines a compensating angle. The compensating angle is amanipulated variable (a rotational amount) for correcting the relativerelationship among the positions of the ground contact portions 10 bythe operation of rotation about a certain point (correcting in thevertical direction in the present embodiment). In the presentembodiment, there are a 14th node compensating angle θ14, a 23rd nodecompensating angle θ23, and a 1423rd node compensating angle θ1423 asthe compensating angles. In other words, they are the compensatingangles of the nodes other than leaf nodes. Then, the hierarchicalcompliance operation determiner 114 determines a desired n-th node floorreaction force central point Qn′ obtained by correcting the desired n-thnode floor reaction force central point Qn (n=1, 2, 3, 4, 14, 23) on thebasis of these compensating angles θ14, θ23 and θ1423.

As shown in FIG. 15, the 14th node compensating angle θ14 is the angleformed by a segment Q1Q4 and a segment Q1′Q4′, and the 23rd nodecompensating angle θ23 is the angle formed by a segment Q2Q3 and asegment Q2′Q3′. Further, as shown in FIG. 16, the 1423rd nodecompensating angle θ1423 is the angle formed by a segment Q14Q23 and asegment Q14′Q23′. The technique for determining these node compensatingangles θ14, θ23 and θ1423 will be described later.

Referring to FIG. 15 and FIG. 16, the correction of the desired n-thnode floor reaction force central point Qn (n=1, 2, 3, 4, 14, 23) on thebasis of the compensating angles θ14, θ23 and θ1423 is made as follows.

Referring to FIG. 15, a normal vector V14 of a plane that includes adesired first node floor reaction force central point Q1 (the desiredfloor reaction force central point of a first ground contact portion 10)and a desired fourth node floor reaction force central point Q4 (thedesired floor reaction force central point of a second ground contactportion 10) and that is perpendicular to a horizontal plane isdetermined. The magnitude of V14 is 1. The coordinate (position) of thedesired first node floor reaction force central point Q1 is rotationallymoved about a normal vector V14 with the desired 14th node floorreaction force central point Q14 as the center of rotation (about theaxis that passes Q14 and is parallel to V14) by the aforesaid 14th nodecompensating angle θ14. The point after Q1 is moved by the aboverotational movement is defined as Q1′. Similarly, the coordinate(position) of the desired fourth node floor reaction force central pointQ4 is rotationally moved about the normal vector V14 by the 14th nodecompensating angle θ14, the desired 14th node floor reaction forcecentral point being the center of rotation. The point after Q4 is movedby the above rotational movement is defined as Q4′. In other words, theends of a segment obtained by rotating the segment Q1Q4 about V14 by θ14with Q14, which is an internally dividing point thereof, as the centerof rotation are defined as Q1′ and Q4′. Thus, the 14th node compensatingangle θ14 is a manipulated variable for moving the relative relationshipof the positions of the desired floor reaction force central points Q1and Q4 of the first node and the fourth node, respectively, which arechild nodes of the 14th node, without moving the position of the desiredfloor reaction force central point Q14 of the 14th node.

Moreover, a normal vector V23 of a plane that includes a desired secondnode floor reaction force central point Q2 and a desired third nodefloor reaction force central point Q3 and that is perpendicular to ahorizontal plane is determined. The magnitude of V23 is 1. Thecoordinate (position) of the desired second node floor reaction forcecentral point Q2 is rotationally moved about the normal vector V23 withthe desired 23rd node floor reaction force central point Q23 as thecenter of rotation (about the axis that passes Q23 and is parallel toV23) by the 23rd node compensating angle θ23. As shown in FIG. 15, thepoint after Q2 is moved by the above rotational movement is defined asQ2′. Similarly, the coordinate (position) of the desired third groundcontact portion floor reaction force central point Q3 is rotationallymoved about the normal vector V23 by the 23rd node compensating angleθ23, the desired 23rd node floor reaction force central point being thecenter of rotation. The point after Q3 is moved by the above rotationalmovement is defined as Q3′. In other words, the ends of a segmentobtained by rotating the segment Q2Q3 about V23 by θ23 with Q23, whichis an internally dividing point thereof, as the center of rotation aredefined as Q2′ and Q3′. Thus, the 23rd node compensating angle θ23 is amanipulated variable for moving the relative relationship of thepositions of the desired floor reaction force central points Q2 and Q3of the second node and the third node, respectively, which are childnodes of the 23rd node, without moving the position of the desired floorreaction force central point Q23 of the 23rd node.

Next, referring to FIG. 16, a normal vector V1423 of a plane thatincludes a desired 14th node floor reaction force central point Q14 anda desired 23rd node floor reaction force central point Q23 and that isperpendicular to a horizontal plane is determined. The magnitude ofV1423 is 1. The coordinate (position) of the desired 14th node floorreaction force central point Q14 is rotationally moved about the normalvector V1423 by the 1423rd compensating angle θ1423, the desired totalfloor reaction force central point P(=Q1423) being the center ofrotation (about the axis that passes P and is parallel to V1423). Thepoint after Q14 is moved by the above rotational movement is defined asQ14′. Similarly, the coordinate (position) of the desired 23rd groundcontact portion floor reaction force central point Q23 is rotationallymoved about the normal vector V1423 by the 1423rd compensating angleθ1423, the desired total floor reaction force central point P being thecenter of rotation. The point after Q23 is moved by the above rotationalmovement is defined as Q23′. In other words, the ends of a segmentobtained by rotating a segment Q14Q23 about V1423 by θ1423, usingP(=Q1423), which is an internally dividing point thereof, as the centerof rotation are defined as Q14′ and Q23′. Thus, the 1423rd nodecompensating angle θ1423 is a manipulated variable for moving therelative relationship of the positions of the desired floor reactionforce central points Q14 and Q23 of the 14th node and the 23rd node,respectively, which are child nodes of the 1423rd node, without movingthe position of the desired floor reaction force central point P of the1423rd node.

Hereinafter, generally, a vector whose start point is A and end point isB will be denoted as a vector A_B.

Next, a point Q1″ that would be obtained when Q1′ is moved by a vectorQ14_Q14′ is determined. Similarly, a point Q4″ that would be obtainedwhen Q4′ is moved by a vector Q14_Q14′ is determined. Further, a pointQ2″ that would be obtained when Q2′ is moved by a vector Q23_Q23′ isdetermined. Similarly, a point Q3″ that would be obtained when Q3′ ismoved by a vector Q23_Q23′ is determined.

Subsequently, the desired ground contact portion position of an n-thground contact portion (n=1, 2, 3, 4) is moved in parallel by a vectorQn_Qn″ (substantially vertical movement). This corrects the desiredground contact portion position of each ground contact portion 10.Supplementally, as with the desired floor reaction force central pointof each node, if the position of each leaf node is set to the positionof the ground contact portion 10 corresponding to the leaf node, and theposition of each node having child nodes is defined as a weightedaverage position of the positions of all child nodes of the node, usingthe weights set as described above, then the processing for moving thedesired floor reaction force central points of the child nodes of the14th node, the 23rd node and the 1423rd node, respectively, according tothe compensating angles θ14, θ23 and θ1423, as described above, isequivalent to moving the positions of the child nodes of the 14th node,the 23rd node and the 1423rd node, respectively, according to thecompensating angles θ14, θ23 and θ1423.

In a robot in which the distal end portions of its legs have no freejoints (a robot in which the postures of ground contact portions can becontrolled), the aforesaid parallel movement is performed withoutchanging the postures (desired postures) of ground contact portions, andthen the n-th ground contact portion is further rotated by a certainrotational angle θn_x about a longitudinal axis (X axis) and alsorotated about a lateral axis (Y axis) by a certain rotational angleθn_y, with Qn″ being the center. This corrects the desired postures ofground contact portions. The rotational angle θn (a two-dimensionalamount composed of θn_x and θn_y) in this case is referred to as an n-thground contact portion compensating angle, θn_x is referred to as ann-th ground contact portion compensating angle X component, and θn_y isreferred to as an n-th ground contact portion compensating angle Ycomponent. The n-th ground contact portion compensating angle θn may bedetermined according to the technique described in Japanese UnexaminedPatent Application Publication No. H10-277969 previously proposed by thepresent applicant.

Supplementally, each of the node compensating angles is determined suchthat a resultant force of a desired floor reaction force moment (thehorizontal component thereof is zero) and a node compensating floorreaction force moment Mn_dmd is generated at the desired node floorreaction force central point of the node, which is the point of action.In this case, the compensating total floor reaction force moment Mdmd,which provides the basis of the node compensating floor reaction forcemoment Mn_dmd, is determined so that an actual posture inclination errorapproximates zero. Hence, each node compensating angle will function asa manipulated variable for manipulating the relative positionalrelationship among the ground contact portions 10 so that an actualposture inclination error approximates zero while bringing a floorreaction force moment about a desired total floor reaction force centralpoint close to a desired moment (a compensating total floor reactionforce moment in this case).

As described above, the processing for correcting the desired groundcontact portion positions (specifically, the mutual relativerelationship among the positions) of the ground contact portions 10 isthe hierarchical compliance operation in the first embodiment. In thecompliance operation, for each node having child nodes, the manipulatedvariable (correction amount) of the relative relationship among thedesired ground contact portion positions (relative positionalrelationship) of the ground contact portions 10, which are thedescendant nodes of the node, is established for each of thecompensating angles θ14, θ23 and θ1423, and the manipulated variables(correction amounts) are combined so as to correct the mutual relativerelationship of the desired ground contact portion positions of theground contact portions 10. The desired ground contact portionposition/posture corrected as described above is referred to as acorrected desired ground contact portion position/posture.

In such a compliance operation, generally, if the aforesaid compensatingangles (compensating operation amounts) are not excessive, then theground contact region (a region of the ground contact surface whereinthe pressure is positive) remains unchanged even if the ground contactpressure distribution of each ground contact portion 10 changes. In thiscase, the compliance mechanism 42 attached to each ground contactportion 10 deforms in proportion to a compensating angle, and an actualfloor reaction force of each ground contact portion 10 based on theamount of the deformation is generated. As a result, the relationshipbetween the compensating angles and the changing amounts of the actualfloor reaction forces generated by the compensating angles has goodcharacteristics, namely, linear characteristics, shown below.

-   Characteristic 1) If only the compensating angles about the desired    floor reaction force central points of nodes that are not leaf nodes    are manipulated to move the desired ground contact portion positions    of the ground contact portions 10, then the translational force    components of the actual floor reaction forces of the ground contact    portions 10 that have been lowered increase, while the translational    force components of the actual floor reaction forces of the ground    contact portions 10 that have been raised decrease. At this time,    the actual floor reaction force moments about the corrected desired    floor reaction force central point of the ground contact portions 10    (leaf nodes) hardly change.-   Characteristic 2) If only an n-th ground contact portion    compensating angle is manipulated to rotate the desired posture of    the n-th ground contact portion, then the moment component of the    actual floor reaction force acting on the desired floor reaction    force central point of the n-th ground contact portion changes,    while the translational force component does not change much.-   Characteristic 3) If the compensating angle about the desired floor    reaction force central point of a node that is not a leaf node and    an n-th ground contact portion compensating angle are simultaneously    manipulated, then the change amount of the actual floor reaction    forces of the ground contact portions 10 will be equivalent to the    sum of the change amount observed when each of them is independently    manipulated.

FIG. 17 is a block diagram showing the functions of the hierarchicalcompliance operation determiner 114 in the present embodiment. Referringto the diagram, the processing of the hierarchical compliance operationdeterminer 114 will be explained in further detail.

The hierarchical compliance operation determiner 114 is equipped with,as its functional means, a compensating total floor reaction forcemoment distributor 114 a, compensating angle determiners 114 b, 114 cand 114 d, a corrected desired ground contact portion position/posturecalculator 114 g, a deformation compensation amount calculator 114 n,and a corrected desired ground contact portion position/posture withdeformation compensation calculator 114 h.

The compensating total floor reaction force moment distributor 114 adistributes the aforesaid compensating total floor reaction force momentMdmd (Mdmdx, Mdmdy) to the 1423rd node compensating floor reaction forcemoment M1423dmd, the 14th node compensating floor reaction force momentM14dmd, and the 23rd node compensating floor reaction force momentM23dmd.

The 1423rd node compensating floor reaction force moment M1423dmd is thedesired value of a moment to be generated about the desired 1423rd nodefloor reaction force central point(=desired total floor reaction forcecentral point (desired ZMP)) by the translational force components ofthe floor reaction forces of the ground contact portions 10(specifically, the first to the fourth ground contact portions) when the1423rd compensating angle θ1423 is manipulated.

The component of the 1423rd node compensating floor reaction forcemoment M1423dmd in the direction of the aforesaid vector V1423 (thecomponent about the axis in the V1423 direction) is described asM1423dmdv. The vector V1423 is the vector defined in the generalexplanation of the compliance operation of the hierarchical complianceoperation determiner 114 (refer to FIG. 16). If a vector orthogonal toV1423 and also orthogonal to the vertical direction is defined as U1423,then the component in the direction of U1423 of the 1423rd nodecompensating floor reaction force moment M1423dmd (the component aboutthe axis in the U1423 direction) M1423dmdu is set to zero in the presentembodiment. This is because the component of a floor reaction forcemoment in the U1423 direction cannot be generated even if the 1423rdnode compensating angle θ1423 is manipulated in the robot 1 of thepresent embodiment. Further, in the present embodiment, the rotation ofthe posture of the robot 1 about the vertical axis is not controlled;therefore, the component M1423dmdz of M1423dmd in the vertical directionis also set to zero.

The 14th node compensating floor reaction force moment M14dmd is thedesired value of a moment to be generated about the desired 14th nodefloor reaction force central point by the translational force componentsof the floor reaction forces of the ground contact portions 10(specifically, the first and the fourth ground contact portions) whenthe 14th compensating angle θ14 is manipulated.

The component of the 14th node compensating floor reaction force momentM14dmd in the direction of the vector V14 is described as M14dmdv. Thevector V14 is the vector defined in the general explanation of thecompliance operation of the hierarchical compliance operation determiner114 (refer to FIG. 15). If a vector orthogonal to V14 and alsoorthogonal to the vertical direction is defined as U14, then thecomponent M14dmdu in the direction of U14 of the 14th node compensatingfloor reaction force moment M14dmd is set to zero in the presentembodiment. This is because the component of a floor reaction forcemoment in the U14 direction cannot be generated even if the 14th nodecompensating angle θ14 is manipulated in the robot 1 of the presentembodiment. In the present embodiment, the component of M14dmd in thevertical direction is also set to zero.

The 23rd node compensating floor reaction force moment M23dmd is thedesired value of a moment to be generated about the desired 23rd nodefloor reaction force central point by the translational force componentsof the floor reaction forces of the ground contact portions 10(specifically, the second and the third ground contact portions) whenthe 23rd compensating angle θ23 is manipulated.

The component of the 23rd node compensating floor reaction force momentM23dmd in the direction of the vector V23 is described as M23dmdv. Thevector V23 is the vector defined in the general explanation of thecompliance operation of the hierarchical compliance operation determiner114 (refer to FIG. 15). If a vector orthogonal to V23 and alsoorthogonal to the vertical direction is defined as U23, then thecomponent M23dmdu in the direction of U23 of the 23rd node compensatingfloor reaction force moment M23dmd is set to zero in the presentembodiment. This is because the component of a floor reaction forcemoment in the U23 direction cannot be generated even if the 23rd nodecompensating angle θ23 is manipulated in the robot 1 of the presentembodiment. In the present embodiment, the component of M23dmd in thevertical direction is also set to zero.

The 1423rd node compensating floor reaction force moment M1423dmd, the14th node compensating floor reaction force moment M14dmd, and the 23rdnode compensating floor reaction force moment M23dmd are determined, forexample, as follows.

On an arbitrary n-th node, when a desired n-th node floor reaction forcecentral point has been corrected while maintaining the horizontalcomponent of an n-th node compensating floor reaction force moment atzero so as to be equivalent to adding the n-th node compensating floorreaction force moment to the desired n-th node floor reaction forcecentral point, the desired n-th node floor reaction force central pointposition that has been corrected is referred to as a corrected desiredn-th node floor reaction force central point position.

The relationships between a corrected desired 1423rd node floor reactionforce central point Pmdfd(=Q1423mdfd), a corrected desired 14th nodefloor reaction force central point Q14mdfd, and a corrected desired 23rdnode floor reaction force central point Q23mdfd, and node compensatingfloor reaction force moments are shown in the following expressions 7 to9. The desired 1423rd node floor reaction force central point Q1423agrees with the total floor reaction force central point P, so that thecorrected desired 1423rd node floor reaction force central point Pmdfdmay be referred to as a corrected desired total floor reaction forcecentral point in some cases.M1423dmd=(Pmdfd−P)*Ftotalref  Expression 7M14dmd=(Q14mdfd−Q14)*F14ref  Expression 8M23dmd=(Q23mdfd−Q23)*F23ref  Expression 9

The difference between two points, such as (Pmdfd−P) in expression 7,means the difference in the positional vectors of the points. Further,Ftotalref, F14ref, and F23ref denote the translational force componentsof the desired node floor reaction forces of the 1423rd node, the 14thnode, and the 23rd node, respectively, as shown in FIG. 10.

To maintain high ground contact properties of the robot 1, controlshould not be carried out to bring the floor reaction force of theground contact portion 10 to be in contact with the ground excessivelyclose to zero or a negative value. Hence, the following conditions 1) to3) should be satisfied.

Corrected Node Existing Position Condition 1)

Pmdfd is not excessively close to an end of the segment Q14Q23 from thetotal floor reaction force central point P and exists on the segmentQ14Q23. The range in which the corrected desired 1423rd node floorreaction force central point (corrected desired total floor reactionforce central point) Pmdfd should exist is referred to as the existencepermissible range of the corrected desired 1423rd node floor reactionforce central point (corrected desired total floor reaction forcecentral point).

Corrected Node Existing Position Condition 2)

Q14mdfd is not excessively close to an end of the segment Q1Q4 from thedesired 14th node floor reaction force central point Q14 and exists onthe segment Q1Q4. The range in which the corrected desired 14th nodefloor reaction force central point Q14mdfd should exist is referred toas the existence permissible range of the corrected desired 14th nodefloor reaction force central point.

Corrected Node Existing Position Condition 3)

Q23mdfd is not excessively close to an end of the segment Q2Q3 from thedesired 23rd node floor reaction force central point Q23 and exists onthe segment Q2Q3. The range in which the corrected desired 23rd nodefloor reaction force central point Q23mdfd should exist is referred toas the existence permissible range of the corrected desired 23rd nodefloor reaction force central point.

Meanwhile, to obtain an appropriate posture restoring force actuallygenerated in the robot 1 (the force for restoring the inclination of thebody 24 to a desired body posture inclination), the resultant force ofthe 1423rd node compensating floor reaction force moment M1423dmd, the14th node compensating floor reaction force moment M14dmd, and the 23rdnode compensating floor reaction force moment M23dmd shouldapproximately agree with the compensating total floor reaction forcemoment Mdmd. In other words, the following expression 10 should beapproximately satisfied.Mdmd=M1423dmd+M14dmd+M23dmd  Expression 10

In the present embodiment, therefore, the 1423rd node compensating floorreaction force moment M1423dmd, the 14th node compensating floorreaction force moment M14dmd, and the 23rd node compensating floorreaction force moment M23dmd are determined according to expressions 11to 13 given below as long as the positions of the corrected desired nodefloor reaction force central points Pmdfd, Q14mdfd, and Q23mdfddetermined by the above and the aforesaid expression 7, expression 8,and expression 9 satisfy the aforesaid corrected node existence positionconditions 1), 2) and 3).M1423dmd=Mat1423*Mdmd*V1423  Expression 11M14dmd=Mat14*Mdmd*V14  Expression 12M23dmd=Mat23*Mdmd*V23  Expression 13where Mat1423, Mat14, and Mat23 denote gain matrixes (1-row, 3-columnmatrixes with a third element being zero), and these are set such thatM1423dmd, M14dmd, and M23dmd determined according to expression 11,expression 12, and expression 13 satisfy expression 10.

More precisely, the gain matrixes Mat1423, Mat14, and Mat23 aredetermined such that a simultaneous equation composed of expression 10,expression 11, expression 12, and expression 13 identically holdsregardless of the value of Mdmd. The gain matrix for the simultaneousequation to identically hold is not uniquely determined, so that anappropriate gain matrix may be determined depending on, for example,which one of the corrected node existence position conditions 1), 2) and3) is especially important. Supplementally, the gain matrixes aredesirably changed continuously in order to continuously change thecompensating angles θ1423, θ14, and θ23. Further, the policy of settinggain matrixes may be changed according to whether the robot 1 isstanding upright or the difference in movement mode or the like.

If the 1423rd node compensating floor reaction force moment M1423dmd,the 14th node compensating floor reaction force moment M14dmd, and the23rd node compensating floor reaction force moment M23dmd determinedaccording to expression 11, expression 12, and expression 13 do notsatisfy any one of the corrected node existence position conditions 1),2) and 3), then they are to be corrected to satisfy the conditions 1),2) and 3). More specifically, of the corrected desired n-th node floorreaction force central points (n=1423, 14, 23), the corrected desirednode floor reaction force central point that has exceeded the existencepermissible range is set on the boundary of the existence permissiblerange, and the remaining corrected desired node floor reaction forcecentral points are determined so as to satisfy expression 11, expression12, and expression 13 as much as possible (to minimize the absolutevalue of the difference between a left side and a right side as much aspossible). Generally, however, the behavior of a controlled object doesnot considerably change even if a feedback amount slightly changes infeedback control; therefore, it is not required to strictly or forciblysatisfy expression 11, expression 12 and expression 13.

Thus, the compensating total floor reaction force moment distributor 114a determines the node compensating floor reaction force momentsM1423dmd, M14dmd, and M23dmd. FIG. 14 shows the examples of the nodecompensating floor reaction force moments M1423dmd, M14dmd, and M23dmddetermined as described above. In the figure, Mdmd is the same as thatshown in FIG. 13 mentioned above.

Moreover, the compensating total floor reaction force moment distributor114 a determines the corrected desired ground contact portion floorreaction forces Fn_refmdfd (n=1, 2, 3, 4), which are the desired floorreaction forces of the ground contact portions 10 that are corrected byadding node compensating floor reaction force moments to desired groundcontact portion floor reaction forces, on the basis of the desiredground contact portion floor reaction forces, which are the desiredfloor reaction forces of the ground contact portions 10 (the desirednode floor reaction forces of leaf nodes) and the node compensatingfloor reaction force moments M1423dmd, M14dmd and M23dmd.

At this time, the corrected desired ground contact portion floorreaction forces Fn_refmdfd (n=1, 2, 3, 4) may be determined from thedesired floor reaction force central points Qn (n=1, 2, 3, 4), thecorrected total floor reaction force central points Pmdfd, the correcteddesired node floor reaction force central points Q14mdfd and Q23mdfd,and the desired total floor reaction forces Ftotalref of the groundcontact portions 10 by the same technique as the technique fordetermining deciding desired node floor reaction forces from the desiredfloor reaction force central points (the desired node floor reactionforce central points of leaf nodes) Qn (n=1, 2, 3, 4), the desired totalfloor reaction force central points P, the desired node floor reactionforce central points Q14 and Q23, and the desired total floor reactionforces Ftotalref of the ground contact portions 10. In other words, theweights of the nodes may be determined according to the aforesaidexpressions 1 to 3 from the desired floor reaction force central pointsQn (n=1, 2, 3, 4), the corrected total floor reaction force centralpoints Pmdfd, the corrected desired node floor reaction force centralpoints Q14mdfd and Q23mdfd of the ground contact portions 10, and thecorrected desired ground contact portion floor reaction forcesFn_refmdfd (n=1, 2, 3, 4) may be determined according to the aforesaidexpression 4 by using the determined weights.

As is obvious from the above, generating a node compensating floorreaction force moment at a desired n-th node floor reaction forcecentral point of an n-th node having child nodes (correcting the momentcomponent of the desired floor reaction force acting on a desired n-thnode floor reaction force central point) is equivalent to correcting theweights of the child nodes of the n-th node.

The processing of the compensating total floor reaction force momentdistributor 114 a explained above is represented by the functional blockdiagram shown in FIG. 18. More specifically, based on the desired nodefloor reaction force central points Qn (n=1, 2, 3, 4, 14, 23), theexistence permissible ranges of the 14th node, the 23rd node, and thecorrected desired node floor reaction force central points Qn_mdfd(n=14, 23, 1423) are determined according to the aforesaid correctednode existence position conditions 1), 2) and 3). Further, the correcteddesired node floor reaction force central points Qn_mdfd (n=14, 23,1423) and the node compensating floor reaction force moments Mn_dmd(n=14, 23, 1423) are determined on the basis of the compensating totalfloor reaction force moment Mdmd, the desired floor reaction forcecentral points of nodes Qn (n=1, 2, 3, 4, 14, 23, 1423), the desiredfloor reaction forces Fn_ref (n=1, 2, 3, 4, 14, 23, 1423), and theaforesaid existence permissible ranges. In addition, the correcteddesired ground contact portion floor reaction forces Fn_refmdfd (n=1, 2,3, 4) are determined on the basis of the corrected desired node floorreaction force central points Qn_mdfd (n=14, 23, 1423), the desiredfloor reaction force central points Qn (n=1, 2, 3, 4) of the groundcontact portions 10, and the desired total floor reaction forceFtotalref.

Next, the processing of the compensating angle determiner (θ1423determiner) 114 b out of the aforesaid compensating angle determiners114 b to 114 d will be explained with reference to the block diagram ofFIG. 19. If the translational force component (F1act+F4act) of theresultant force of the actual first node floor reaction force and theactual fourth node floor reaction force acts on a desired 14th nodefloor reaction force central point Q14, and the translational forcecomponent (F2act+F3act) of the resultant force of the actual second nodefloor reaction force and the actual third node floor reaction force actson a desired 23rd node floor reaction force central point Q23, then amoment M1423act generated about the desired total floor reaction forcecentral point P (the desired 1423rd node floor reaction force centralpoint) by the above translational force components is determinedaccording to expression 14 given below.M1423act=P _(—) Q14*(F1act+F4act)+P _(—) Q23*(F2act+F3act)  Expression14where P_Q14 denotes a vector whose start point is P and end point isQ14, and P_Q23 denotes a vector whose start point is P and end point isQ23.

In actuality, there will be hardly a problem if expression 15 givenbelow is used in place of expression 14 to calculate M1423act.M1423act=P _(—) Q1*F1act+P _(—) Q2*F2act+P _(—) Q3*F3act+P _(—)Q4*F4act  Expression 15

The right side of expression 15 is an expression for calculating theactual total floor reaction force moment Mtotalact acting about thedesired total floor reaction force central point P from thetranslational force components Fn_act (n=1, 2, 3, 4) of the actual floorreaction forces of the first to the fourth nodes (leaf nodes).Supplementally, in expression 14, the actual floor reaction force momentacting about the desired 14th node floor reaction force central pointQ14 and the actual floor reaction force moment acting about the desired23rd node floor reaction force central point Q23 are subtracted from theactual total floor reaction force moment Mtotalact acting about thedesired total floor reaction force central point P.

In general, an actual n-th node floor reaction force moment Mn_act of anarbitrary n-th node, which is a leaf node, is the actual floor reactionforce moment of an n-th ground contact portion. Regarding an arbitraryn-th node, which is not a leaf node, the moment from the floor reactionforces of all child nodes thereof acting on the desired n-th node floorreaction force central point (the floor reaction forces of the childnodes here are, strictly speaking, the floor reaction forces acting onthe desired floor reaction force central points of the child nodes) isreferred to as an actual n-th node floor reaction force moment Mn_act.As in expression 14 and expression 15, there are two differentdefinitions of an actual n-th node floor reaction force moment. Oneincludes the actual floor reaction force moments of child nodes and theother does not include them; however, either of the definitions may beused. Especially when the responsiveness of feedback control forcontrolling the horizontal components of the actual floor reaction forcemoments of child nodes is high, the horizontal components of the actualfloor reaction force moments of the child nodes immediately converge tozero, so that the behavior of the control of the actual floor reactionforce of an n-th node does not change much when either of thedefinitions is used.

Expression 16 shown below is a general expression for calculating anactual n-th node floor reaction force moment, which corresponds toexpression 14, and expression 17 is a general expression for calculatingan actual n-th node floor reaction force moment, which corresponds toexpression 15. In expression 16 and expression 17, Qn_Qm is a vectorwhose start point is Qn and end point is Qm.

An actual floor reaction force moment Mm_act of an m-th node, which is aleaf node, is an actual floor reaction force moment of each groundcontact portion detected by an actual floor reaction force detector.

when m∈ {set of numbers of child nodes of n-th node},Mn_act=Σ(Qn _(—) Qm*Fm_act)  Expression 16when m∈ {set of numbers of leaf nodes, which are descendants of n-thnode},Mn_act=Σ(Qn _(—) Qm*Fm_act+Mm_act)  Expression 17

Σ in expressions 16 and 17 means the total sum regarding m.Supplementally, in the first embodiment, the actual floor reaction forcemoment about the desired floor reaction force central point of a leafnode (each ground contact portion) becomes zero; therefore, the rightside of the above expression 15 does not include a component of Mm_actof expression 17. In a robot in which the postures of ground contactportions can be controlled, Mm_act of expression 17 generally does notbecome zero.

The 1423rd node compensating angle θ1423 may generally be determined byfeedback control law or the like such that the difference betweenM1423act determined as described above and the 1423rd node compensatingfloor reaction force moment M1423dmd previously determined by thecompensating total floor reaction force moment distributor 114 a(M1423act−M1423dmd) approximates zero. For example, θ1423 may bedetermined by, for example, multiplying the difference by apredetermined gain matrix (tertiary diagonal matrix).

However, in the present embodiment, the 1423rd node compensating angleθ1423 about the axis in the direction of the aforesaid vector V1423 maybe determined, so that θ1423 may be determined on the basis of thedifference between a component M1423actv in the direction of the vectorV1423 of M1423act and a component M1423dmdv in the direction of thevector V1423 of M1423dmd. And, at this time, in the present embodiment,M1423actv and M1423dmdv are passed through a filter before determiningθ1423 on the basis of the difference therebetween in order to enhancethe responsiveness and stability of the control of floor reactionforces.

Specifically, the component M1423actv in the direction of the vectorV1423 of M1423act determined as described above is extracted. This isobtained by the following expression 18 using a scalar productcalculation “·” of a vector.M1423actv=M1423act·V1423  Expression 18

The processing for calculating M1423actv as described above is executedby a calculator indicated by a reference numeral 114 k in FIG. 19.

Next, the aforesaid M1423actv is passed through a low-pass filter 114 ito obtain M1423actvfilt. Furthermore, the component M1423dmdv in theV1423 direction of the aforesaid 1423rd node compensating floor reactionforce moment M1423dmd is passed through a compensating filter 114 j toobtain M1423dmdvfilt. Incidentally, M1423dmdv is determined by thescalar product calculation of M1423dmd and V1423, as with the aforesaidexpression 18. Then, the result obtained by subtracting M1423dmdvfiltfrom M1423actvfilt provides a component M1423errv in the direction of anerror moment V1423.

The aforesaid compensating filter 114 j improves the frequency responsecharacteristic of transfer functions from M1423dmdv to an actual totalfloor reaction force moment in a control device.

Lastly, the aforesaid 1423rd node compensating angle θ1423 is obtainedby the calculation of the feedback control law (proportional control lawin this case) of the following expression 19. Here, K1423 denotes acontrol gain, and it is normally set to a positive value.θ1423=K1423*M1423errv  Expression 19This means that the component M1423errv in direction of the error momentV1423 is multiplied by the control gain K1423 to obtain the 1423rd nodecompensating angle θ1423.

The processing of the compensating angle determiner (θ14 determiner) 114c in FIG. 17 will now be explained with reference to FIG. 20. If thetranslational force component F1act of the actual first node floorreaction force (the actual floor reaction force of the first groundcontact portion 10) acts on a desired first node floor reaction forcecentral point Q1, and the translational force component F4act of theactual fourth node floor reaction force (the actual floor reaction forceof the fourth ground contact portion 10) acts on a desired fourth nodefloor reaction force central point Q4, then a moment M14act generatedabout the desired 14th node floor reaction force central point Q14 bythe above translational force components is determined according to theaforesaid expression 16, which is a general expression.

More specifically, it is determined according to the followingexpression 20.M14act=Q14_(—) Q1*F1act+Q14_(—) Q4*F4act  Expression 20where Q14_Q1 denotes a vector whose start point is Q14 and end point isQ1, and Q14_Q4 denotes a vector whose start point is Q14 and end pointis Q4.

In actuality, for the reason described previously, there will be hardlya problem if the following expression 21 (expression substantiating theabove expression 17) is used in place of expression 20.M14act=Q14_(—) Q1*F1act+Q14_(—) Q4*F4act+M1act+M4act  Expression 21where M1act denotes an actual first node floor reaction force moment andM4act denotes an actual fourth node floor reaction force moment. In thefirst embodiment, the distal portions of the legs #1 to #4 are providedwith free joints (spherical joints 12); hence, M1act and M4act are zero.

Expression 21 is an expression for calculating the moment acting aboutthe desired 14th node floor reaction force central point generated bythe resultant force of the actual floor reaction forces of all leafnodes of the 14th node. Incidentally, expression 20 subtracts the actualfirst node floor reaction force moment and the actual fourth node floorreaction force moment from the moment acting about the desired 14thfloor reaction force central point generated by the resultant force ofthe actual floor reaction forces of all leaf nodes of the 14th node.

The 14th node compensating angle θ14 may be generally determined by thefeedback control law or the like such that the difference between M14actdetermined as described above and the 14th node compensating floorreaction force moment M1423dmd previously determined by the compensatingtotal floor reaction force moment distributor 114 a (M14act−M14dmd)approximate zero.

In the present embodiment, however, for the same reason as that in thecase of θ1423, θ14 is determined on the basis of the difference betweenthe component M14actv of M14act in the aforesaid direction of the vectorV14 and the component M14dmdv of M14dmd in the direction of the vectorV14, which have been respectively passed through a filter.

More specifically, the component M14actv of M14act in the direction ofthe vector V14 determined as described above is extracted. This isobtained by expression 22 that uses the scalar product calculation ofvectors.M14actv=M14act·V14  Expression 22

The processing for calculating M14actv as described above is carried outby a calculator indicated by a reference character 114 k′ in FIG. 20.

Next, the aforesaid M14actv is passed through a low-pass filter 114 i′to obtain M14actvfilt. Further, the component M14dmdv of the aforesaid14th node compensating floor reaction force moment in the direction ofV14 is passed through a compensating filter 114 j′ to obtainM14dmdvfilt. Incidentally, M14dmdv is determined by the scalar productcalculation of M14dmd and V14. Then, M14dmdvfilt is subtracted fromM14actvfilt to obtain a component M14errv in the direction of an errormoment V14. Incidentally, the compensating filter 114 j′ improves thefrequency response characteristics of transfer functions from M14dmdv toactual total floor reaction force moments in a control device.

Lastly, the aforesaid 14th node compensating angle θ14 is obtained bythe calculation of the feedback control law (proportional control law inthis case) of expression 23 given below. Here, K14 denotes a controlgain, which is normally set to a positive value.θ14=K14*M14errv  Expression 23

The processing of the compensating angle determiner (θ23 determiner) 114d in FIG. 17 is the same as the processing of the 14th node compensatingangle θ14 determiner 114 c; therefore, detailed explanation thereof willbe omitted here. The following provides an overview of the processing. Acomponent M23actv of a moment M23act in the direction of a vector V23,the moment being generated about a desired 23rd node floor reactionforce central point Q23 by the actual floor reaction forces of the childnodes of a 23rd node, is calculated on the basis of the aforesaidexpression 16 or 17. Then, the 23rd node compensating angle θ23 iscalculated by the arithmetic processing of the feedback control law(proportional control law) from a component M23errv in the direction ofthe error moment V23 obtained by subtracting M23actvfilt, which isobtained by passing the above M23actv through a low-pass filter, fromM23dmdvfilt obtained by passing M23dmdv of the aforesaid 23rd nodecompensating floor reaction force moment M23dmd in the direction of V23through a compensating filter.

The processing of the compensating angle determiners 114 b to 114 ddetermines the sets of the node compensating angles θ1423, θ14 and θ23such that the horizontal components of the actual moments Mact acting onthe desired total floor reaction force central points P approximate thecompensating total floor reaction force moment Mdmd. In the presentembodiment, θ1423 has been determined by the aforesaid expression 19;alternatively, however, it may be determined on the basis of thedifference between M1423act+M14act+M23act (the total sum of actual nodefloor reaction force moments) and M1423dmd or the difference between thetotal sum of actual node floor reaction force moments and M1423dmd,which have been respectively passed through a filter, in place ofM1423err of the right side of expression 19.

Supplementally, in the present embodiment, a node compensating floorreaction force moment has been determined without changing the desirednode floor reaction force central point Qn of each node, the Qn beingthe point of action. Then, the difference between the node floorreaction force compensating moment (more precisely, the resultant forceof the node compensating floor reaction force moment and the momentcomponent of the desired node floor reaction force having Qn as itspoint of action) and an actual node floor reaction force moment havingQn as its point of action has been used as a control amount to determinea node compensating angle such that the control amount approximateszero. Instead of determining the node compensating angles as describedabove, the node compensating angles θ1423, θ14 and θ23 may be determinedas follows. On each n-th node (n=14, 23, 1423) having child nodes, afloor reaction force central point that causes the horizontal componentof the moment of the actual node floor reaction force of the n-th node(the resultant force of the actual node floor reaction forces of allchild nodes of the n-th node) to become zero is determined as an actualn-th node floor reaction force central point. Alternatively, a floorreaction force central point that causes the horizontal component of amoment, which is obtained by subtracting the moment acting on thedesired node floor reaction force central point of each child node dueto the actual node floor reaction force of the child node from themoment of the actual node floor reaction force of an n-th node (n=14,23, 1423), to become zero is determined as an actual n-th node floorreaction force central point. For example, the actual 14th node floorreaction force central point related to the 14th node is determined as apoint obtained by shifting, on the segment Q1Q4, the desired 14th nodefloor reaction force central point by the value obtained by dividing thehorizontal component of M14act determined by the aforesaid expression 16or 17 by the vertical component of the resultant force of F1act andF4act (the translational force component of an actual 14th node floorreaction force). The same applies to the node floor reaction forcecentral points of the 23rd node and the 1423rd node. Then, thedifference between the desired node floor reaction force central pointof an n-th node (n=14, 23, 1423) and the actual n-th node floor reactionforce central point determined as described above, or the differencebetween the values obtained by passing each of the above node floorreaction force central points through a filter is defined as a controlamount, and the node compensating angles θ1423, θ14, and θ23 aredetermined on the basis of the control amount such that the controlamount approximates zero (e.g., θ1423, θ14 and θ23 are determined bymultiplying the control amount by a certain gain).

The corrected desired ground contact portion position/posture calculator114 g in FIG. 17 obtains corrected desired ground contact portionposition/posture by correcting the desired ground contact portionposition/posture, which are the desired position and the desired postureof each ground contact portion 10, according to a technique forcorrecting the desired ground contact portion position/posture of theaforesaid hierarchical compliance operation (the technique explainedwith reference to FIG. 15 and FIG. 16) on the basis of the 1423rd nodecompensating angle θ1423, the 14th node compensating angle θ14, and the23rd node compensating angle θ23. However, in the present embodiment,the distal portions of the legs #1 to #4 are provided with free joints(spherical joints 12), and the posture of each ground contact portion 10cannot be intentionally changed; therefore, the corrected desired groundcontact portion position/posture actually means the corrected desiredground contact portion position.

FIG. 21 is a functional block diagram showing the processing of theaforesaid deformation compensation amount calculator 114 n in FIG. 17.As shown in FIG. 21, the deformation compensation amount calculator 114n determines deformation amounts En_mdfd (n=1, 2, 3, 4), which are thedeformation amounts of the legs #1 to #4 (the deformation amounts of thelink mechanisms of the legs and the compliance mechanism 42), thedeformation being expected to occur due to the corrected desired groundcontact portion floor reaction forces Fn_refmdfd (n=1, 2, 3, 4) of theground contact portions 10 that are output from the compensating totalfloor reaction force moment distributor 114 a (the desired groundcontact portion floor reaction forces that have been corrected by addingthe 1423rd node compensating floor reaction force moment M1423dmd, the14th node compensating floor reaction force moment M14dmd, and the 23rdnode compensating floor reaction force moment M23dmd to desired groundcontact portion floor reaction forces). The deformation amounts En_mdfd(n=1, 2, 3, 4) are determined by using a mechanism compliance modelshowing the relationship between the forces (or floor reaction forces)acting on the legs and the deformations of the legs. Then, thedeformation compensation amount calculator 114 n determines deformationcompensation amounts En_cmpn (n=1, 2, 3, 4) for canceling thedeformation amounts En_mdfd (n=1, 2, 3, 4). The deformation compensationamount En_cmpn is determined by multiplying each deformation amountEn_mdfd by (−1).

The corrected desired ground contact portion position/posture withdeformation compensation calculator 114 h in FIG. 17 adds a furthercorrection to the corrected desired ground contact portionposition/posture of each ground contact portion 10 (the position/posturedetermined by the aforesaid corrected desired ground contact portionposition/posture calculator 114 g) so as to cancel the calculateddeformation amount En_mdfd, thereby obtaining the corrected desiredground contact portion position/posture with deformation compensation ofeach ground contact portion 10. The corrected desired ground contactportion positions/postures with deformation compensation are determinedby adding individually corresponding deformation compensation amountsEn_cmpn to the corrected desired ground contact portion position/postureof the ground contact portions 10.

For example, if a calculation result indicates that a corrected desiredground contact portion floor reaction force would cause the compliancemechanism 42 or the like of a leg corresponding to a certain groundcontact portion 10 to contract by z in the vertical direction, then thecorrected desired ground contact portion floor reaction force iscorrected so as to lower the desired position of the ground contactportion 10 by z. More specifically, the corrected desired ground contactportion position/posture with deformation compensation is calculatedsuch that the position/posture when the ground contact surface (bottomsurface) of the ground contact portion 10 after deformation compensationis deformed under a desired ground contact portion floor reaction forceagrees with the desired position/posture of the ground contact surfaceof the ground contact portion before the deformation compensation.Incidentally, detailed explanation thereof is explained in detail inJapanese Unexamined Patent Application Publication No. H10-277969. Inthe robot 1 of the present embodiment, the postures of the groundcontact portions 10 cannot be controlled, so that the corrected desiredground contact portion position/posture with deformation compensationcalculator 114 h actually corrects the corrected desired ground contactportion positions of the ground contact portions 10.

The deformation compensation described above is implemented by controlfor canceling in a feed-forward manner the shifts in actual groundcontact portion position/posture caused by a deformation of thecompliance mechanism 42 or the like. In comparison with a case where nosuch control is carried out, it is possible to enable the robot 1 totravel in a gait that is further closer to a desired gait.

The above has given the details of the processing of the hierarchicalcompliance operation determiner 114.

Based on the above, resuming the explanation of the flowchart in FIG. 9,compensating angles are determined as described above in S34. FIG. 22 isa flowchart showing a generalized subroutine of the processing fordetermining the compensating angles.

To explain with reference to the figure, first, the translational forcecomponents Fn_act (n=1, 2, 3, 4, 14, 23, 1423) of actual n-th node floorreaction forces are calculated on the basis of the actual floor reactionforces of the ground contact portions 10 (the actual floor reactionforces obtained by the aforesaid actual floor reaction force detector108). In this case, the translational force components Fn_act (n=1, 2,3, 4) of the actual node floor reaction forces of the leaf nodes are thetranslational force components of the detected values of the actualfloor reaction forces of the ground contact portions 10 by the aforesaidactual floor reaction force detector 108. Further, the translationalforce component Fn_act (n=14, 23, 1423) of the actual node floorreaction force of a node that is not a leaf node is the translationalforce component of the resultant force of the actual node floor reactionforces of the child nodes of the node, as described above.

Subsequently, the processing proceeds to S102 to calculate an actualn-th node floor reaction force moment Mn_act (n=1, 2, 3, 4, 14, 23,1423). In the robot 1 of the first embodiment, the actual node floorreaction force moment Mn_act (n=1, 2, 3, 4) of each leaf node is zero.Further, the actual node floor reaction force moment Mn_act (n=14, 23,1423) of each node that is not a leaf node is calculated on the basis ofthe aforesaid expression 16 or expression 17 from Fn_act (n=1, 2, 3, 4)obtained in S100 and the desired node floor reaction force central pointQn (n=1, 2, 3, 4, 14, 23, 1423).

Subsequently, the processing proceeds to S104 wherein the n-th nodecompensating floor reaction force moment Mn_dmd (n=14, 23, 1423) isdetermined on the basis of the compensating total floor reaction forcemoment Mdmd determined by the aforesaid posture stabilization controlcalculator 104. This processing is carried out as described above by theaforesaid compensating total floor reaction force moment distributor 114a.

Subsequently, the processing proceeds to S106 to determine the vectorsV1423, V14 and V23 defined in the explanation of the hierarchicalcompliance operation and vectors U1423, U14 and U23 orthogonal thereto.In the first embodiment, however, the floor reaction force moments inthe directions of the vectors U1423, U14 and U23 cannot be generated;therefore, it is unnecessary to determine U1423, U14 and U23.

Supplementally, as in the second embodiment to be described hereinafter,if the number of child nodes of an n-th node is 3 or more, then Vn maybe set in any direction as long as it does not suddenly change timewise;hence, the direction of Vn may be appropriately determined by, forexample, setting it to the direction of the X axis of a supporting legcoordinate system or the direction of the body of a robot. Further, Unis to be orthogonal to Vn.

Subsequently, the processing proceeds to S108 to extract a componentMn_actv in the Vn direction and a component Mn_actu in the Un directionof an actual n-th node floor reaction force moment Mn_act (n=14, 23,1423). This may be accomplished by carrying out the scalar productcalculation of Mn_act and Vn and Un. Incidentally, if the number ofchild nodes of an arbitrary n-th node that is not a leaf node is two orless, as in the first embodiment, then the component Mn_actu in the Undirection is zero. Hence, it is unnecessary to determine Mn_actu.

Subsequently, the processing proceeds to S110 to extract a componentMn_dmdv in the Vn direction and a component Mn_dmdu in the Un directionof an n-th node compensating floor reaction force moment Mn_dmd (n=14,23, 1423). This may be accomplished by carrying out the scalar productcalculation of Mn_dmd and Vn and Un. Incidentally, if the number ofchild nodes of an arbitrary n-th node that is not a leaf node is two orless, as in the first embodiment, then the component Mn_dmdu in the Undirection of the n-th node compensating floor reaction force momentMn_dmd is set to zero. Alternatively, it is unnecessary to determineMn_dmdu.

Subsequently, the processing proceeds to S112 to determine a Vncomponent of an n-th node compensating angle θn by multiplying thedifference between a value obtained by passing Mn_actv through a filterand a value obtained by passing Mn_dmdv through a filter by a gain Kn(more generally, according to the feedback control law from thedifference). In the first embodiment, this processing is carried out asdescribed above on the 14th node, the 23rd node, and the 1423rd node bythe aforesaid compensating angle determiners 114 b, 114 c and 114 d.

Subsequently, the processing proceeds to S114 to determine a Uncomponent of an n-th node compensating angle θn by multiplying thedifference between a value obtained by passing Mn_actu through a filterand a value obtained by passing Mn_dmdu through a filter by the gain Kn(more generally, according to the feedback control law from thedifference). However, if the number of child nodes of an arbitrary n-thnode that is not a leaf node is two or less, as in the first embodiment,then an n-th node compensating angle U component is set to zero.

Alternatively, it is unnecessary to carry out the processing of S114.

The above is the subroutine processing of S34 in FIG. 9. Supplementally,the processing of S106 to S114 may be regarded as the processing fordetermining an n-th node compensating angle such that the actual n-thnode floor reaction force moment acting on a desired n-th node floorreaction force central point converges to an n-th node compensatingfloor reaction force moment (more precisely, the resultant force of then-th node compensating floor reaction force moment and the desired nodefloor reaction force moment acting on the desired n-th node floorreaction force central point).

Subsequently, the processing proceeds to S36 of the flowchart of FIG. 9to calculate the aforesaid deformation compensation amount. Thisprocessing is carried out by the aforesaid deformation compensationamount calculator 114 n as described above.

Subsequently, the processing proceeds to S38 to correct a desired groundcontact portion position/posture on the basis of the compensating angledetermined in S34 and to further correct it on the basis of thedeformation compensation amount determined in S36, thereby obtaining thecorrected desired ground contact portion position/posture withdeformation compensation of each ground contact portion 10. In the firstembodiment, the corrected desired ground contact portion positions ofthe ground contact portions 10 are determined on the basis of thecompensating angles θ1423, θ14, and θ23 as described above (as explainedwith reference to the aforesaid FIG. 15 and FIG. 16) by the correcteddesired ground contact portion position/posture calculator 114 g. Then,the determined corrected desired ground contact portion positions arefurther corrected by the corrected desired ground contact portionposition/posture with deformation compensation calculator 114 h on thebasis of the aforesaid deformation compensation amount En_cmpn(n=1, 2,3, 4), as described above, thereby obtaining the corrected desiredground contact portion positions with deformation compensations of theground contact portions 10.

The processing from S32 to S38 in FIG. 9 explained above is theprocessing of the hierarchical compliance operation determiner 114.

Subsequently, the processing proceeds to S40 to calculate the jointdisplacement command of the robot 1 from the desired bodyposition/posture and the corrected ground contact portionposition/posture with deformation compensation (the corrected groundcontact portion position with a deformation compensation in the firstembodiment). This processing is carried out as described above by theaforesaid robot geometric model 110.

Subsequently, the processing proceeds to S42 to make an actual jointdisplacement follow the joint displacement command. This follow-upcontrol (servo control) is carried out by the aforesaid displacementcontroller 112.

Subsequently, the processing proceeds to S44 to update time by Δt (thearithmetic processing cycle of the control device 50), and returns toS14 to repeat the processing described above.

Owing to the hierarchical compliance operation as in the firstembodiment explained above, the control of each node floor reactionforce hardly interferes with each other, thus allowing the node floorreaction forces to be controlled easily and properly also in a mobilerobot having three or more ground contact portions. This restrainscontrol interference and also restrains the actual floor reaction forceof each node from deviating from a desirable value or from vibrating.Thus, even if a floor surface has not only an undulation or tilt over alarge area thereof but also unexpected changes in the floorconfiguration, including local projections or depressions or slopes,floor reaction forces acting on a legged mobile robot can be properlycontrolled without being influenced by them much. Moreover, control forstabilizing the posture of a mobile robot can be easily accomplished andthe landing shocks to the mobile robot can be reduced, thus allowing theground contact properties of the mobile robot to be enhanced and alsopreventing the mobile robot from slipping or spinning when it travels.Furthermore, the loads of the actuators of the mobile robot can bereduced. Hence, the floor reaction forces of ground contact portions canbe properly controlled, so that high posture stability can be obtained.

To supplement the advantages contributing to the stabilization of theposture of a robot, when the entire robot inclines from a state in whichthe robot is walking as expected on an expected floor surface, therelationship between an inclination angle error θberr (θberrx, θberry)and an increasing amount ΔM of the moment horizontal component about adesired total floor reaction force central point generated in responsethereto is preferably a proportional relationship. If not, it is stillpreferred that expression 24 given below holds for a certain rotationalmatrix T and a certain diagonal matrix diag(a,b). Incidentally, T,diag(a,b) are secondary square matrixes.T*ΔM=diag(a,b)*T*θberr  Expression 24

If these relationships are not satisfied, then the inclination angleerror θberr will not linearly converge when the robot restores itsposture from the inclined state, and a precession movement may occur.For instance, in a state wherein the body of a robot inclines forward,the force (restoring force) to fall sideways other than the restoringforce for returning backward excessively acts, and the inclination angleerror θberr does not linearly return to zero. Instead, a restoring forceacts toward back sideways and the inclination angle error θberr spirallyconverges to zero.

For the same reason as that described above, in the compliance control,the relationship between a changing rate dθberr/dt of the inclinationangle error θberr of the entire robot and an increasing amount ΔM_d ofthe moment generated in response thereto is also preferably aproportional relationship. If not, it is still preferred that expression25 given below holds for a certain rotational matrix T and a certaindiagonal matrix diag(e,f). Incidentally, T, diag(e,f) are secondarysquare matrixes.T*ΔM _(—) d=diag(e,f)*T*dθberr/dt  Expression 25

In general, if compliance control is independently carried out on eachleg of a robot, these relationships may not be satisfied and aprecession movement may take place. In the hierarchical compliancecontrol shown in the first embodiment, the relationships of theaforesaid expressions 24 and 25 can be satisfied, so that theconvergence of the posture control of the robot is high and it ispossible to prevent oscillation or vibration.

In the aforesaid first embodiment, the desired ground contact portionpositions have been compensated (corrected) so as to rotationally moveabout node floor reaction force central points without changing thepostures of the ground contact portions (without controlling thepostures), thereby correcting the relative heights of the ground contactportions 10 from the relative heights of the desired ground contactportion positions. Alternatively, however, only the heights of theground contact portions 10 may be corrected by moving the desired groundcontact portion positions only in the vertical direction. Specifically,a desired ground contact portion position is corrected according to thefollowing procedure.

First, by the following expressions 26 and 27, a vertical positioncorrection amount Z14 of a desired 14th node floor reaction forcecentral point and a vertical position correction amount Z23 of a desired23rd node floor reaction force central point are determined.Z14=−PQ14*θ1423  Expression 26Z23=PQ23*θ1423  Expression 27Here, note that the value determined by the aforesaid expression 19 issubstituted into θ1423.

Next, the vertical position correction amounts Zn (n=1, 2, 3, 4) of thedesired floor reaction force central points of the ground contactportions (the desired node floor reaction force central points of leafnodes) are determined according to the following expressions.Z1=−Q14Q1*θ14+Z14  Expression 28Z4=Q14Q4*θ14+Z14  Expression 29Z2=−Q23Q2*θ23+Z23  Expression 30Z3=Q23Q3*θ23+Z23  Expression 31Here, note that the value determined by the aforesaid expression 23 issubstituted into θ14, and the value determined in the same manner asthat for θ14 is substituted into θ23.

The corrected desired ground contact portion positions are obtained byadding Z1, Z2, Z3 and Z4 determined as described above to the desiredground contact portion positions in the vertical direction.

Further, in the aforesaid first embodiment, the ground contact portions10 have been hierarchized as shown in the aforesaid FIG. 6; however, thehierarchical structure does not necessarily have to be decided to be onehierarchical structure beforehand. For example, the hierarchicalstructure may be changed according to the travel mode (the motion modeof legs when traveling) of the robot 1, including trotting or galloping.For instance, the ground contact portions 10 may be hierarchized asshown in FIG. 23. FIG. 23 illustrates, as in the aforesaid FIG. 3( b),an example wherein the pair of the first ground contact portion 10 andthe second ground contact portion 10 is defined as a 12th node and thepair of the third ground contact portion 10 and the fourth groundcontact portion 10 is defined as a 34th node in a period during whichall legs #1 to #4 of the robot 1 are all supporting legs. In thisexample, the desired node floor reaction force central points Qn (n=1,2, 3, 4, 12, 34, 1234) of the nodes are set as illustrated. With thisarrangement, the aforesaid compliance operation and the estimation of afloor configuration, which will be discussed later, can be performedmore accurately in some cases.

Second Embodiment

The following will explain a second embodiment of the present inventionwith reference to the aforesaid FIG. 1 and FIG. 24 to FIG. 34. Referringto FIG. 1, the explanation will be focused on the aspects of a robot 1of the second embodiment that are different from the robot of the firstembodiment. The robot 1 of the second embodiment is provided with, inaddition to the first to the fourth legs #1 to #4, a fifth leg #5 and asixth leg #6 having the same structures as those of the legs #1 to #4.This means that the robot 1 of the second embodiment is a six-leggedrobot. The fifth leg #5 is behind the third leg #3 and extended from theright side of the body 24 of the robot 1, and the sixth leg #6 is behindthe fourth leg #4 and extended from the left side of the body 24 of therobot 1. The rest of the mechanical structure of the robot 1 is the sameas that of the first embodiment, so that the like reference numerals asthose of the first embodiment will be assigned and the explanationthereof will be omitted.

The basic concept of the technique of the hierarchical compliancecontrol of the robot 1 (six-legged robot) of the second embodiment isthe same as that of the first embodiment. However, the hierarchicalcompliance control of the second embodiment differs from that of thefirst embodiment in that there is a node having three child nodes andcontrol processing is expanded to handle them.

The following will explain the control processing of the robot 1 of thesecond embodiment, focusing mainly on the different aspects. Regardingthe reference numerals and terms used in the explanation of the secondembodiment, the same reference numerals and terms as those in the firstembodiment will be used for those that have the equivalent meanings asthose in the first embodiment, and detailed explanation thereof will beomitted.

FIG. 24 is a diagram for explaining the hierarchical structure in thesecond embodiment, and it is a diagram corresponding to FIG. 3( b) inthe first embodiment. As shown in FIG. 24, in the second embodiment, thesix individual ground contact portions 10 are defined as leaf nodes (thefirst to the sixth nodes), and the set of all the six ground contactportions 10 is defined as a root node (the 145236th node). In addition,the set of the first, the fourth, and the fifth ground contact portions10, which are the ground contact portions of the first leg #1, thefourth leg #4, and the fifth leg #5 is defined as a 145th node, and theset of the second, the third, and the sixth ground contact portions 10,which are the ground contact portions of the second leg #2, the thirdleg #3, and the sixth leg #6 is defined as a 236th node. In other words,the 145th node is an intermediate node having the first, the fourth, andthe fifth nodes (three leaf nodes) as child nodes, and the 236th node isan intermediate node having the second, the third, and the sixth node(three leaf nodes) as child nodes.

In the second embodiment, for example, the set of the three groundcontact portions 10 of the 145th node and the set of the three groundcontact portions 10 of the 236th node are alternately lifted and landedto move the robot 1. FIG. 24 shows the state wherein both sets arelanded (the state wherein both sets are supporting legs).

The triangles with the reference marks Qn (n=1, 2, 3, 4, 5, 6) in thefigure correspond to the positions of the ground contact portions 10.

Incidentally, Qn (n=1, 2, 3, 4, 5, 6) denotes the desired floor reactionforce central point of each of the first to the sixth ground contactportions 10, which are leaf nodes (desired node floor reaction forcecentral points), Q145 and Q236 denote the desired floor reaction forcecentral points of the 145th node and the 236th node, respectively(desired node floor reaction force central points), and P denotes thedesired total floor reaction force central point(=desired ZMP) as thedesired floor reaction force central point (desired node floor reactionforce central point) Q145236 of the root node (the 145236th node). Aswith the first embodiment, Qn (n=1, 2, 3, 4, 5, 6) agrees with thecentral point of each corresponding ground contact portion 10.

The overall functional construction of a control device 50 in the secondembodiment is the same as that shown in FIG. 2 explained in conjunctionwith the aforesaid first embodiment.

In this case, a gait generating device 100 in the second embodimentdetermines and outputs desired motion trajectories (a desired groundcontact portion trajectory and a desired body position/posturetrajectory) of the robot 1 and desired floor reaction force trajectories(a desired total floor reaction force central point trajectory and adesired total floor reaction force trajectory), as with that in thefirst embodiment. In the second embodiment, however, the desired groundcontact portion trajectory is the trajectory of the desired groundcontact portion position of each of the six ground contact portions 10.If the ground contact portions are provided such that their postures arecontrollable, then the trajectory of desired ground contact portionpostures is also included in the desired ground contact portiontrajectory. Further, the desired total floor reaction force centralpoint trajectories are determined according to the motion modes of thelegs #1 to #6 (especially the positions at which supporting legs areexpected to land) in the second embodiment such that they continuouslymove while existing in a range, in which ZMPs can exist, at positionsnot excessively close to the boundaries of the range (e.g., at anapproximately central position of the range wherein a ZMP can exist).

A desired floor reaction force distributor 102 in the second embodimentdetermines desired node floor reaction force central points, the weightsof nodes, and desired node floor reaction forces such that the followingconditions A′) to G′) are satisfied, as with the first embodiment.

-   A′) The desired node floor reaction force central point Qn (n=1, 2,    3, 4, 5, 6) of each leaf node agrees with the central point of the    ground contact portion 10 corresponding to the leaf node. More    generally, Qn (n=1, 2, 3, 4, 5, 6) is determined according to a    desired gait (a desired motion, such as a desired ground contact    portion trajectory). For example, if the desired floor reaction    force central point of each ground contact portion 10 is determined    by the gait generating device 100, then the desired node floor    reaction force central point Qn (n=1, 2, 3, 4, 5, 6) may be    determined on the basis of the desired floor reaction force    determined by the gait generating device 100.-   B′) The desired node floor reaction force central point of the root    node agrees with a desired total floor reaction force central point    P.-   C′) The desired node floor reaction force central point Qn of an    arbitrary n-th node (n=145, 236, 145236) having child nodes will be    a weighted average point of the desired node floor reaction force    central points of the child nodes of the n-th node. More    specifically, a desired node floor reaction force central point Q145    of a 145th node provides the internally dividing point of a triangle    having, as its apexes, desired node floor reaction force central    points Q1, Q4 and Q5 of the first, the fourth, and the fifth nodes,    which are the child nodes (leaf nodes) thereof (points on a boundary    of the triangle being included), and a desired node floor reaction    force central point Q236 of a 236th node provides the internally    dividing point of a triangle having, as its apexes, desired node    floor reaction force central points Q2, Q3 and Q6 of the second, the    third, and the sixth nodes, which are the child nodes (leaf nodes)    thereof (points on a boundary of the triangle being included).    Further, a desired node floor reaction force central point    Q145236(=desired total floor reaction force central point P) of a    145236th node (root node) provides an internally dividing point of a    segment connecting the desired node floor reaction force central    points Q145 and Q236 of the 145th node and the 236th node, which are    the child nodes (intermediate nodes) thereof.-   D′) The desired node floor reaction force Fn of an arbitrary n-th    node (n=145, 236, 145236) having child nodes agrees with the sum    (resultant force) of the desired node floor reaction forces of all    child nodes of the n-th node, and the desired node floor reaction    force F145236 of the root node (the 145236th node) agrees with the    desired total floor reaction force Ftotalref. Alternatively, the    desired node floor reaction force Fn (n=1, 2, 3, 4, 5, 6, 145,    236, 145236) of each node has the relationship with the weight of    each node indicated by the aforesaid expression 4a or 4b.-   E′) The desired node floor reaction force of a leaf node associated    with the ground contact portion 10 not in contact with the ground is    zero.-   F′) The desired node floor reaction force central point, the weight,    and the desired node floor reaction force of each node (n-th node    (n=1, 2, 3, 4, 145, 236, 145236)) continuously change.-   G′) The weight of a leaf node corresponding to the ground contact    portion 10 in a no-contact-with-ground state or the weight of any    one of the ancestor nodes of the leaf node is zero.

The weight of the root node has been set to “1” for the sake ofconvenience, as with the aforesaid first embodiment.

The processing of a posture error calculator 103 and a posturestabilization control calculator 104 in the second embodiment is thesame as that in the first embodiment; a compensating total floorreaction force moment Mdmd (Mdmdx, Mdmdy) is determined as explained inthe first embodiment.

An actual floor reaction force detector 108 in the second embodiment, asthat in the first embodiment, detects the actual floor reaction forcesacting on the ground contact portions 10 (the six ground contactportions in the present embodiment) from the outputs of six-axis forcesensors 34 provided on the legs #1 to #6 and converts them into actualfloor reaction forces represented by a supporting leg coordinate system(the global coordinate system shown in FIG. 1, which is fixed to afloor).

Primarily on the basis of the final desired trajectories of groundcontact portion positions and desired body position/posture, a robotgeometric model (inverse kinematics calculator) 110 in the secondembodiment calculates joint displacement commands of the robot 1 thatsatisfy them by inverse kinematics calculation, as that in the firstembodiment.

Further, a displacement controller 112 in the second embodiment controlsactuators (not shown) of the joints 14 and 15 of the robot 1 so as tomake actual joint displacements follow the aforesaid joint displacementcommands, as that in the first embodiment.

Further, a hierarchical compliance operation determiner 114 in thesecond embodiment corrects the desired ground contact portiontrajectories of the ground contact portions 10 so as to satisfy theaforesaid requirements 1) and 2) as much as possible, as that in thefirst embodiment. The present embodiment, however, has intermediatenodes, each having three child nodes, so that the specific processing ofthe hierarchical compliance operation determiner 114 is somewhat morecomplicated than that in the first embodiment. If a robot is constructedsuch that the postures of the ground contact portions 10 arecontrollable, then the hierarchical compliance operation determiner 114should correct the desired ground contact portion trajectories of theground contact portions 10 to also satisfy the aforesaid requirement 3)as much as possible.

FIG. 25 is a block diagram showing the functions of the hierarchicalcompliance operation determiner 114 of the second embodiment, and itcorresponds to FIG. 17 in the first embodiment. Referring to FIG. 25,the hierarchical compliance operation determiner 114 of the secondembodiment is equipped with, as its components (functional means), acompensating total floor reaction force moment distributor 114 a,compensating angle determiners 114 b, 114 c and 114 d, a correcteddesired ground contact portion position/posture calculator 114 g, adeformation compensation amount calculator 114 n, and a correcteddesired ground contact portion position/posture with deformationcompensation calculator 114 h, as with the first embodiment.Incidentally, in the second embodiment, compensating angles include a145236th compensating angle θ145236, a 145th compensating angle θ145,and a 236th compensating angle θ236 related to the 145236th node, the145th node, and the 236th node, respectively, and the compensating angledeterminers 114 b, 114 c, and 114 d determine θ145236, θ145, and θ236,respectively.

The following will explain the processing of the hierarchical complianceoperation determiner 114 in the second embodiment, focusing mainly onthe aspects different from the first embodiment.

The hierarchical compliance operation determiner 114 in the secondembodiment, as that in the first embodiment, determines thetranslational force component and the moment component of the desirednode floor reaction force of each node primarily on the basis of anoutput of the desired floor reaction force distributor 102, and alsodetermines the translational force component and the moment component ofthe actual node floor reaction force of each node on the basis of anoutput of the actual floor reaction force detector 108.

More specifically, referring to FIG. 26, a translational force componentFn_ref (n=1, 2, 3, 4, 5, 6) of the desired node floor reaction force ofeach leaf node is identical to the translational force component of thedesired node floor reaction force determined by the desired floorreaction force distributor 102, and a translational force componentF145236ref of the desired node floor reaction force of the root node isidentical to a translational force component Ftotalref of the desiredtotal floor reaction force determined by the gait generating device 100.Further, a translational force component Fn_ref (n=145, 236) of adesired node floor reaction force of an intermediate node other thanleaf nodes and the root node is determined to be the translational forcecomponent of the resultant force of the desired node floor reactionforces of the child nodes of the intermediate node(F145ref=F1ref+F4ref+F5ref, F236ref=F2ref+F3ref+F6ref). In this case,the parent node of the intermediate node is the root node, so thatF145ref+F236ref=Ftotalref. Incidentally, FIG. 26 illustrates thetranslational force component Fn_ref of the desired node floor reactionforce of each node in a state wherein all the ground contact portions 10of the robot 1 in the second embodiment are in contact with the ground.

The moment component of the desired node floor reaction force of node(more precisely, the moment component of the desired node floor reactionforce having the desired node floor reaction force central point of thenode as its point of action) is always set to zero.

Referring to FIG. 30, a translational force component Fn_act (n=1, 2, 3,4, 5, 6) of the actual node floor reaction force of each leaf node isidentical to the translational force component of the actual floorreaction force of each ground contact portion 10 detected by the actualfloor reaction force detector 108. Further, a translational forcecomponent Fn_act (n=145, 236, 145236) of the actual node floor reactionforce of each node having child nodes is determined to be thetranslational force component of the resultant force of the actual nodefloor reaction forces of the child nodes of the node. Therefore,F145act=F1act+F4act+F5act, F236act=F2act+F3act+F6act,F145236act(=Ftotalact)=F145act+F236act. FIG. 30 illustrates thetranslational force component Fn_act of the actual node floor reactionforce of each node in the state wherein all the ground contact portions10 of the robot 1 in the second embodiment are in contact with theground.

Further, referring to FIG. 31, a moment component of the actual nodefloor reaction force of each leaf node (a moment component having thedesired node floor reaction force central point of the leaf node as itspoint of action) Mn_act (n=1, 2, 3, 4, 5, 6) is basically identical tothe moment component of the actual floor reaction force of each groundcontact portion 10 detected by the actual floor reaction force detector108. In the present embodiment, however, no actual moment occurs at thecentral point of each ground contact portion 10, i.e., the desired floorreaction force central point of the ground contact portion 10;therefore, the moment component of the actual node floor reaction forceof each leaf node is set to zero. Further, regarding the momentcomponent Mn_act of the actual node floor reaction force of each n-thnode (n=145, 236, 145236) that has child nodes, the resultant force ofthe actual node floor reaction forces of the child nodes of the n-thnode is basically determined to be the moment component (this generallydoes not become zero) acting on the desired node floor reaction forcecentral point of the n-th node.

The compensating total floor reaction force moment distributor 114 a inthe second embodiment distributes the aforesaid compensating total floorreaction force moment Mdmd (Mdmdx, Mdmdy) to a 145236th nodecompensating floor reaction force moment M145236dmd, a 145th nodecompensating floor reaction force moment M145dmd, and a 236th nodecompensating floor reaction force moment M236dmd.

The 145236th node compensating floor reaction force moment M145236dmd isthe desired value of the moment to be generated about a desired totalfloor reaction force central point P (desired ZMP) by the translationalforce component of the floor reaction force of each ground contactportion 10 generated by manipulating the 145236th compensating angleθ145236 (by rotating the set of the first, the fourth, and the fifthground contact portions and the set of the second, the third, and thesixth ground contact portions about a desired total floor reaction forcecentral point P(=Q145236) by θ145236).

The 145th node compensating floor reaction force moment M145dmd is thedesired value of the moment to be generated about a desired 145th nodefloor reaction force central point by the translational force componentof the floor reaction force of each ground contact portion 10(specifically, the first, the fourth, and the fifth ground contactportions) generated by manipulating the 145th compensating angle θ145(by rotating the first, the fourth, and the fifth ground contactportions 10 belonging to the 145th node about a desired 145th node floorreaction force central point Q145 by θ145).

The 236th node compensating floor reaction force moment M236dmd is thedesired value of the moment to be generated about a desired 236th nodefloor reaction force central point by the translational force componentof the floor reaction force of each ground contact portion 10(specifically, the second, the third, and the sixth ground contactportions) generated by manipulating the 236th compensating angle θ236(by rotating the second, the third, and the sixth ground contactportions 10 belonging to the 236th node about a desired 236th node floorreaction force central point Q236 by θ236).

In the present embodiment, as with the first embodiment, the posture ofthe robot 1 about the vertical axis is not controlled (because thecomponent of the compensating total floor reaction force moment Mdmdabout the vertical axis is zero), so that M145236dmd, M145dmd, andM236dmd are all moments (horizontal vectors) whose components about thevertical axis are zero, and the compensating angles θ145236, θ145, andθ236 are rotational angles about the horizontal axis. In particular,M145236dmd is the moment about the horizontal axis perpendicular to asegment Q145Q236.

These node compensating floor reaction force moments M145236dmd,M145dmd, and M236dmd are basically determined such that the followingconditions 11) and 12) are satisfied.

-   11) On the 145th node, the 236th node, and the 145236th node, as    defined in the aforesaid first embodiment, when corrected desired    node floor reaction force central points Q145mdfd, Q236mdfd, and    Pmdfd(=Q145236mdfd) that satisfy the relational expressions of the    following expressions 7a to 9a are introduced, these Q145mdfd,    Q236mdfd, and Pmdfd exist in their respective predetermined    existence permissible range.    M145236dmd=(Pmdfd−P)*Ftotalref  Expression 7a    M145dmd=(Q145mdfd−Q145)*F145ref  Expression 8a    M236dmd=(Q236mdfd−Q236)*F236ref  Expression 9a

The existence permissible ranges of Q14bmdfd, Q236mdfd, and Pmdfd areset, for example, as shown in FIG. 29( a) in a state wherein all theground contact portions 10 of the robot 1 of the second embodiment arein contact with the ground. More specifically, the existence permissiblerange of Q145mdfd is the region on the triangle in the bold line in thefigure (the sides of and the region in the triangle), and this is theregion set in the triangle having, as its apexes, the desired node floorreaction force central points Q1, Q4, and Q5 of the child nodes of the145th node such that it is not excessively close to the boundary of thetriangle Q1Q4Q5. The existence permissible range of Q236mdfd is similarto the above. Further, the existence permissible range of Pmdfd is theregion on the segment in bold line in the figure, and this is the regionset on a segment Q145Q236 connecting the desired floor reaction forcecentral points Q145 and 236 of the child nodes of the 145236th node(root node) such that it is not excessively close to the end points ofthe segment Q145Q236.

-   12) Mdmd=M145236dmd+M145dmd+M236dmd is substantially satisfied.

Further, in the present embodiment, the root node (the 145236th node)has two child nodes, so that M145236dmd is limited to a vector in thesame direction as that of a horizontal unit vector (this being denotedby V145236) orthogonal to the segment Q145Q236, as with each nodecompensating floor reaction force moment in the aforesaid firstembodiment. Hence, in the present embodiment, M145236dmd, M145dmd, andM236dmd are determined such that they satisfy the following condition13).

-   13) The component of M145236dmd+M145dmd+M236dmd in the direction of    a vector V145236 takes a value close to the component of Mdmd in the    direction of the vector V145236 as much as possible.

M145236dmd, M145dmd, and M236dmd satisfying these conditions 11) to 13)are determined, for example, as follows. First, M145236dmd is determinedto be the component of Mdmd in the V145236 direction. However, if thecorrected desired node floor reaction force central point Pmdfddetermined by the aforesaid 7a does not fall within its existencepermissible range, then M145236dmd is corrected such that Pmdfd becomesa point of a boundary of the existence permissible range. Subsequently,Mdmd145 and Mdmd236 are determined such that the vector obtained bysubtracting M145236dmd determined as described above from Mdmdsubstantially agrees with the sum of Mdmd145 and Mdmd236, and thecomponent of Mdmd145+Mdmd23 in the V145236 direction is close to theV145236 component of Mdmd-Mdmd145236 as much as possible and alsosatisfies the aforesaid condition 11). In this case, Mdmd145 and Mdmd236are vectors that are parallel to each other.

Examples of M145236dmd, M145dmd, and M236dmd determined as describedabove are shown in FIG. 29( b).

Supplementally, M145236dmd is the horizontal vector perpendicular to thesegment Q145Q236 as described above.

If the posture of the robot 1 about the vertical axis is alsocontrolled, the vertical components of M145236dmd, M145dmd, and M236dmdmay be also determined.

Compensating angle determiners 114 b to 114 d in the second embodimentbasically determine node compensating angles θ145236, θ145, and θ236 onthe basis of the difference between the node compensating floor reactionforce moment and the actual node floor reaction force moment (the momenthaving a desired node floor reaction force central point as the point ofaction) of each n-th node (n=145236, 145, 236) such that the differenceapproximates zero. The examples are shown in FIG. 30 and FIG. 31. Inthis case, in the present embodiment, θ145 is the angle of rotation inthe same direction as the difference M145act−M145dmd and about an axisthat passes the desired 145th node floor reaction force central pointQ145, as shown in FIGS. 30, and θ236 is the angle of rotation in thesame direction as the difference M236act−M236dmd and about an axis thatpasses the desired 236th node floor reaction force central point Q236,as shown in FIG. 30. Further, θ145236 is the angle of rotation in thesame direction as the difference M145236act−M145236dmd (the horizontaldirection perpendicular to the segment Q145Q236) and about an axis thatpasses the desired total floor reaction force central point P, as shownin FIG. 31.

However, in the present embodiment also, as with the first embodiment,the node compensating angles θ145236, θ145, and θ236 are determined onthe basis of the difference between the node compensating floor reactionforce moment of an n-th node (n=145236, 145, 236) that has been passedthrough a filter and the actual node floor reaction force moment thathas been passed through a filter.

FIG. 32 is a block diagram showing the functions of the compensatingangle determiner 114 b (θ145236 determiner) that determines the nodecompensating angle θ145236 as described above, and FIG. 33 is a blockdiagram showing the functions of the compensating angle determiner 114 c(θ145 determiner) that determines the node compensating angle θ145 asdescribed above. The processing of the compensating angle determiner 114d (θ236 determiner) is the same as the processing of the θ145 determiner114 b, so that it will neither be explained nor shown.

The compensating angle determiner (θ145236 determiner) 114 b firstdetermines the moment M145236act generated about the desired total floorreaction force central point P (the desired 145236th node floor reactionforce central point) according to the aforesaid expression 16 or 17 ifthe translational force component (F1act+F4act+F5act) of the resultantforce of actual first, fourth, and fifth node floor reaction forces actson the desired 145th node floor reaction force central point Q145, andthe translational force component (F2act+F3act+F6act) of the resultantforce of actual second, third, and sixth node floor reaction forces actson the desired 236th node floor reaction force central point Q236. Then,a component M145236actv of the determined M145236act in the direction ofa vector V145236 is determined by scalar product calculation. Here, thevector V145236 is a horizontal unit vector that is perpendicular to thesegment Q145Q236. Regarding the 145236th node, it is unnecessary todetermine the component of M145236act in a vector U145236 directionbecause the positions of the desired floor reaction force central pointsof the child nodes (the 145th node and the 236th node) cannot bemanipulated about the axis in the direction of a horizontal unit vectorU145236 perpendicular to V145236.

Subsequently, as with the first embodiment, a differenceM145236errv(=M145236actvfilt−M145236dmdvfilt) between M145236actvfiltobtained by passing the M145236actv through a low-pass filter andM145236dmdvfilt obtained by passing a component M145236dmdv of theaforesaid 145236th node compensating floor reaction force momentM145236dmd in the direction of a vector V through a compensation filteris multiplied by a predetermined gain K145236 so as to determine the145236th node compensating angle θ145236.

The compensating angle determiner (θ145 determiner) 114 c firstcalculates a moment M145act generated about the 145th node floorreaction force central point Q145 when translational force componentsF1act, F4act, and F5act of actual first, fourth, and fifth node floorreaction forces act on desired first, fourth, and fifth node floorreaction force central points Q1, Q4, and Q5, respectively. In thiscase, M145act to be calculated is formed of components in the directionsof vectors V145 and U145, respectively, which are horizontal unitvectors that are orthogonal to each other. The directions of the vectorsV145 or U145 may be arbitrary.

Subsequently, a difference M145err(=M145actfilt−M145dmdfilt) betweenM145actfilt obtained by passing the M145act through a low-pass filterand M145dmdfilt obtained by passing the aforesaid 145th nodecompensating floor reaction force moment M145dmd (a component in thevector V direction and a component in the vector U direction) through acompensation filter is multiplied by a predetermined gain matrix K145(diagonal matrix) so as to determine the 145th node compensating angleθ145. θ145 is constructed of an angle component about the axis of thevector V and an angle component about the axis of the vector U.

The processing for determining the 236th node compensating angle θ236 bythe compensating angle determiner (θ236 determiner) 114 d is carried outin the same manner as the processing of the aforesaid θ145 determiner114 c.

The processing by the compensating angle determiners 114 b to 114 dexplained above determines a set of the node compensating anglesθ145236, θ145, and θ236 such that the horizontal component of the actualmoment Mact acting on the desired total floor reaction force centralpoint P approximates the compensating total floor reaction force momentMdmd.

As supplementally explained in conjunction with the first embodiment,the node compensating angles θ145236, θ145, and θ236 may be determinedas follows. For each n-th node (n=145, 236, 145236) having child nodes,a floor reaction force central point that causes the horizontalcomponent of the moment of the actual node floor reaction force of then-th node (the resultant force of the actual node floor reaction forcesof all child nodes of the n-th node) to be zero is determined as anactual n-th node floor reaction force central point. Alternatively, afloor reaction force central point that causes the horizontal componentof the moment, which is obtained by subtracting the moment acting on adesired node floor reaction force central point of each node that isgenerated by the actual node floor reaction force of the node from themoment of the actual node floor reaction force of an n-th node (n=145,236, 145236), to be zero is determined as an actual n-th node floorreaction force central point. Then, the difference between the desirednode floor reaction force central point of the n-th node (n=145, 236,145236) and the actual n-th node floor reaction force central pointdetermined as described above, or the difference between the valuesobtained by passing each of those node floor reaction force centralpoints through a filter is defined as a control amount, and the nodecompensating angles θ145236, θ145, and θ236 are determined on the basisof the control amount (e.g., θ145236, θ145, and θ236 are determined bymultiplying the control amount by a certain gain) so that the controlamount approximates zero.

A corrected desired ground contact portion position/posture calculator114 g in the second embodiment shown in FIG. 25 corrects the desiredground contact portion position/posture (actually the desired groundcontact portion position in the robot shown in FIG. 1) of each groundcontact portion 10 so as to obtain a corrected desired ground contactportion position/posture. More specifically, referring to FIG. 30 andFIG. 31, the desired floor reaction force central points Q1, Q4, and Q5of the first, the fourth, and the fifth nodes, respectively, which arethe child nodes of the 145th node, are rotationally moved by the 145thnode compensating angle θ145 (horizontal vector), the desired floorreaction force central point Q145 of the 145th node being the center ofrotation. The Q1, Q4, and Q5 after the rotational movement are denotedby Q1′, Q4′, and Q5′, respectively. Thus, the 145th node compensatingangle θ145 is the manipulated variable for moving the relativerelationship among the positions of the desired floor reaction forcecentral points Q1, Q4, and Q5 of the first, the fourth, and the fifthnodes, which are the child nodes of the 145th node, without moving theposition of the desired floor reaction force central point Q145 of the145th node.

Similarly, the desired floor reaction force central points Q2, Q3, andQ6 of the second, the third, and the sixth nodes, respectively, whichare the child nodes of the 236th node, are rotationally moved by the236th node compensating angle θ236 (horizontal vector), the desiredfloor reaction force central point Q236 of the 236th node being thecenter of rotation. The Q2, Q3, and Q6 after the rotational movement aredenoted by Q2′, Q3′, and Q6′, respectively. Thus, the 236th nodecompensating angle θ236 is the manipulated variable for moving therelative relationship among the positions of the desired floor reactionforce central points Q2, Q3, and Q6 of the second, the third, and thesixth nodes, which are the child nodes of the 236th node, without movingthe position of the desired floor reaction force central point Q236 ofthe 236th node.

These rotational movements are visually shown in FIG. 30.

Subsequently, the desired floor reaction force central points Q145 andQ236 of the 145th and the 236th nodes, which are the child nodes of the145236th node, are rotationally moved by the 145236th node compensatingangle θ145236 about the axial center in the same direction (horizontaldirection orthogonal to a segment Q145Q236) as that of the vector(horizontal vector) of the aforesaid error M145236 errv, the desiredfloor reaction force central point P (desired total floor reaction forcecentral point) of the 145236th node being the center of rotation. TheQ145 and Q236 after the rotational movement are denoted by Q145′ andQ236′, respectively, as shown in FIG. 31. Thus, the 145236th nodecompensating angle θ145236 is the manipulated variable for moving therelative relationship between the positions of the desired floorreaction force central points Q145 and Q236 of the 145th and the 236thnodes, which are the child nodes of the 145236th node, without movingthe position of the desired floor reaction force central point P of the145236th node (root node).

Next, referring to FIG. 31, the desired node floor reaction forcecentral points Q1′, Q4′, and Q5′ after the previous rotational movementare moved in parallel by a vector Q145_Q145′. This provides finalcorrected desired node floor reaction force central points Q1″, Q4″, andQ5″ of the first, the fourth, and the fifth nodes. Similarly, thedesired node floor reaction force central points Q2′, Q3′, and Q6′ afterthe previous rotational movement are moved in parallel by a vectorQ236_Q236′. This provides final corrected desired node floor reactionforce central points Q2″, Q3″, and Q6″ of the second, the third, and thesixth nodes.

Lastly, the desired ground contact portion position of an n-th groundcontact portion (n=1, 2, 3, 4, 5, 6) is moved in parallel by a vectorQn_Qn″. This corrects the desired ground contact portion position ofeach ground contact portion 10 (more precisely, the relativerelationship among the desired ground contact portion positions of theground contact portions 10). More specifically, for each node havingchild nodes, the manipulated variable (correction amount) of therelative relationship (relative positional relationship) among thedesired ground contact portion positions of the ground contact portions10, which are the descendant nodes of the node, is determined on thebasis of the compensating angles θ145, θ236, and θ145236, and combiningthe manipulated variables (correction amounts) corrects the mutualrelative relationship among the desired ground contact portion positionsof the ground contact portions 10.

In a robot in which the postures of ground contact portions arecontrollable and floor reaction force moments can be generated about thedesired floor reaction force central points of the ground contactportions, the operation of a foot posture rotation about a desiredground contact portion floor reaction force central point (desired nodefloor reaction force central point) of each ground contact portion maybe performed by the technique shown in Japanese Unexamined PatentApplication Publication No. H10-277969 (composite-compliance control).More specifically, as supplementally explained in conjunction with thefirst embodiment, the desired posture of the n-th ground contact portionmay be corrected about Q″ after the n-th ground contact portion is movedin parallel as described above.

A deformation compensation amount calculator 114 n in the secondembodiment shown in FIG. 25 determines, as with that in the aforesaidfirst embodiment, a deformation compensation amount En_cmpn (n=1, 2, 3,4, 5, 6) for compensating for an influence caused by the deformation ofthe compliance mechanism 42 of each of the legs #1 to #6. Morespecifically, the deformation compensation amount calculator 114 ndetermines a deformation amount En_mdfd (n=1, 2, 3, 4, 5, 6) of thecompliance mechanism 42 or the like of the legs #1 to #6 expected tooccur due to a corrected desired ground contact portion floor reactionforce Fn_refmdfd (n=1, 2, 3, 4, 5, 6) of each ground contact portion 10(the desired ground contact portion floor reaction force corrected byadding the node compensating floor reaction force moments M145236 dmd,M145 dmd, and M236 dmd to a desired ground contact portion floorreaction force) output from the compensating total floor reaction forcemoment distributor 114 a in the second embodiment by using a mechanismcompliance model. This En_mdfd is multiplied by (−1) to determine thedeformation compensation amount En_cmpn.

The corrected desired ground contact portion position/posture withdeformation compensation calculator 114 h in the second embodiment shownin FIG. 25 determines, as with that in the first embodiment, thecorrected desired ground contact portion position/posture withdeformation compensation by adding a corresponding deformationcompensation amount En_cmpn to the corrected desired ground contactportion position/posture (the position/posture determined by theaforesaid corrected desired ground contact portion position/posturecalculator 114 g) of each ground contact portion 10.

The above is the detailed explanation of the processing of thehierarchical compliance operation determiner 114 in the presentembodiment (the second embodiment).

The arithmetic processing of the control device 50 other than thatexplained above is the same as that of the first embodiment.

Incidentally, the hierarchical structure in the second embodiment may bealtered according to an operation mode (traveling mode) or the like ofthe robot 1, as in the case of the first embodiment. For example, thehierarchical structure may be set as shown in FIG. 34. The example inthe figure is equipped with, as intermediate nodes, a 12th node having afirst ground contact portion and a second ground contact portion aschild nodes (leaf nodes), a 34th node having a third ground contactportion and a fourth ground contact portion as child nodes (leaf nodes),a 56th node having a fifth ground contact portion and a sixth groundcontact portion as child nodes (leaf nodes), and a 3456th node having a56th node and a 34th node as child nodes, the root node having the3456th node and the 12th node as child nodes. This arrangement makes itpossible to perform compliance operations and floor configurationestimations, which will be discussed hereinafter, more accurately insome cases. Incidentally, the meanings of the reference characters inthe figure are the same as those shown in, for example, the aforesaidFIG. 3( b) or FIG. 24.

Third Embodiment

An explanation will now be given of a third embodiment in which a floorconfiguration estimating function and a function for correcting theoperation of the robot 1 on the basis of the result of the estimationhave been added to the robot 1 (six-legged robot) of the secondembodiment. In the present embodiment, the explanation will be focusedmainly on the six-legged robot shown in the second embodiment; however,supplemental explanation will be added also to a four-legged robot insome cases. In addition, for the convenience of the understanding of thepresent embodiment, a two-legged robot will be referred to in somecases.

The mechanical construction of the robot 1 in the present embodiment isthe same as that shown in FIG. 1 explained in the first or the secondembodiment (it should be noted that the robot 1 has six legs #1 to #6).Hence, the explanation of the mechanical construction of the robot 1will be omitted. The functional construction of a control device 50provided on the robot 1 in the present embodiment is also the same asthat shown in the aforesaid FIG. 2. In the present embodiment, however,the hierarchical compliance operation determiner 114 in FIG. 2 has newlyadded functions, making it different from that in the second embodiment.Further, the processing of the components of the control device 50except for the hierarchical compliance operation determiner 114 is thesame as that of the second embodiment. Thus, the explanation of thepresent embodiment will be focused mainly on the processing of thehierarchical compliance operation determiner 114, and a detailedexplanation of the processing of the control device 50 other than thatwill be omitted.

FIG. 35 is a block diagram showing the processing functions of thehierarchical compliance operation determiner 114 in the presentembodiment. Of the functions, those functions that are different fromthose in the second embodiment will be explained. A floor configurationestimator 130 serving as a floor configuration estimating means and anadder 132 that adds an estimated floor configuration error (morespecifically, the estimated value of floor height error, which will bediscussed later, related to each ground contact portion 10) output(estimated) by the floor configuration estimator 130 to correcteddesired ground contact portion position/posture have been newly added tothe hierarchical compliance operation determiner 114 in the presentembodiment, and outputs of the adder 132 in place of corrected desiredground contact portion position/posture are supplied to the correcteddesired ground contact portion position/posture with deformationcompensation calculator 114 h. The rest of the processing of thecomponents of the hierarchical compliance operation determiner 114 isthe same as the processing thereof in the second embodiment.

Accordingly, the control processing of the main routine of the controldevice 50 in the present embodiment is partly different from the controlprocessing shown in the flowchart of the aforesaid FIG. 9. FIG. 36 is aflowchart showing the control processing of the main routine of thecontrol device 50 in the present embodiment. As illustrated, in thepresent embodiment, the processing for estimating a floor configurationerror (the processing of the floor configuration estimator 130) in S37is newly added after S36. Furthermore, in S38′, desired ground contactportion position/posture are corrected on the basis of the compensatingangles θ145236, θ145, and θ236 explained in the aforesaid secondembodiment and the floor configuration error estimated in S37, and thedesired ground contact portion position/posture after the correction isfurther corrected on the basis of a deformation compensation amount soas to obtain corrected desired ground contact portion position/posturewith deformation compensation, which are final desired ground contactportion position/posture. In this case, more specifically, correcteddesired ground contact portion position/posture are determined on thebasis of the compensating angles θ145236, θ145, and θ236, as in thesecond embodiment, then the corrected desired ground contact portionposition/posture are corrected on the basis of a floor configurationerror, and they are further corrected on the basis of a deformationcompensation amount, thereby obtaining corrected desired ground contactportion position/posture with deformation compensation. The items otherthan the above are the same as those in the processing of FIG. 9.

Hereinafter, the aspects of the present embodiment that are differentfrom the second embodiment will be specifically explained.

Before starting the detailed explanation of the present embodiment, theconcepts and terms to be used when the floor configuration estimator 130performs estimation will be defined as follows. In the explanation here,for the sake of convenience, schematic diagrams of an average robot,rather than limiting to the six-legged robot 1 in the presentembodiment, will be used.

As shown in FIG. 37, FIG. 38, and FIG. 39, a floor (or a floor surface)supposed in a desired gait is referred to as “supposed floor.” An actualfloor on which a robot travels is referred to as “actual floor.” For theconvenience of explanation, FIG. 37 shows the four-legged robotexplained in the first embodiment, and FIG. 38 and FIG. 39 show atwo-legged robot. The meanings of the terms explained below remain thesame in any multi-legged robots, including the six-legged robot 1 in thepresent embodiment.

The desired n-th ground contact portion floor reaction force centralpoint Qn defined in the hierarchical compliance control explained in theaforesaid first and the second embodiments has been the point set at thecentral point of an n-th ground contact portion; however, the floorreaction force central point Qn may alternatively be set on the groundcontact surface (bottom surface) of the n-th ground contact portion. Inthis case, in the desired gait, the point on the supposed floor surfacethat is supposed to be in contact with the desired n-th ground contactportion floor reaction force central point Qn is referred to as“supposed n-th floor contact point Dn.”

As is obvious from the definition, during the period in which the n-thground contact portion is in contact with the ground in the desired gaitof a robot, the desired n-th ground contact portion floor reaction forcecentral point Qn and the supposed n-th floor contact point Dn share thesame coordinates as observed from a supporting leg coordinate system(global coordinate system). In comparison to this, the point at whichthe point that corresponds to the desired n-th ground contact portionfloor reaction force central point Qn on the bottom surface of an actualn-th ground contact portion when the robot 1 is actually traveling comesin contact with an actual floor is referred to as “actual n-th floorcontact point Dnact.”

Examples showing the relationships among these points are shown in FIG.37, FIG. 38, and FIG. 39. Incidentally, FIG. 37 shows a four-leggedrobot viewed from the direction of the normal line of a vertical planethat passes a desired first ground contact portion floor reaction forcecentral point Q1 and a desired second ground contact portion floorreaction force central point Q2 (that is, substantially sideways), FIG.38 shows a traveling (walking) two-legged robot viewed from thedirection of the normal line of a vertical plane that passes the desiredfirst ground contact portion floor reaction force central point Q1 andthe desired second ground contact portion floor reaction force centralpoint Q2 (that is, substantially sideways), and FIG. 39 shows atwo-legged robot substantially in an upright posture viewed from thedirection of the normal line of a vertical plane that passes the desiredfirst ground contact portion floor reaction force central point Q1 andthe desired second ground contact portion floor reaction force centralpoint Q2 (that is, substantially from rear).

In these FIG. 37 to FIG. 39, the sections of the supposed floors on theaforesaid vertical planes are indicated by thin lines, while thesections of actual floors in the aforesaid vertical planes are indicatedby thick lines. Incidentally, in FIG. 37, the desired posture of therobot (the entire posture of the robot at an instantaneous value of adesired gait) and the actual posture are indicated by dashed lines andsolid lines, respectively. Further, in FIG. 38 and FIG. 39, the desiredpostures of the robots (the entire postures of the robots atinstantaneous values of desired gaits) and actual ground contact portionposition/posture are indicated by thin lines and thick lines,respectively. Actual n-th floor contact points in these situations arethe points on actual floor surfaces, and they are at the positions shownin FIG. 37, FIG. 38, and FIG. 39.

The configuration difference of an actual floor surface with respect toa supposed floor surface is referred to as a floor configuration error.As the indexes for quantitatively representing floor configurationerrors, an n-th node floor height error and an n-th node floorinclination error are defined as follows.

The height of a floor surface at an n-th floor contact point is referredto as “n-th ground contact portion floor height.” In relation to an n-thnode that is a leaf, the difference between an actual n-th groundcontact portion floor height and a supposed n-th ground contact portionfloor height is referred to as “n-th ground contact portion floor heighterror” or “n-th node floor height error.” The inclination of the floorsurface at an n-th floor contact point is referred to as “n-th groundcontact portion floor inclination.” In relation to an n-th node that isa leaf, the difference between an actual n-th ground contact portionfloor inclination and a supposed n-th ground contact portion floorinclination is referred to as “n-th ground contact portion floorinclination error” or “n-th node floor inclination error.” An example ofthe ground contact portion floor inclination error is shown in FIG. 39.

With respect to all j-th nodes, which are leaf nodes, the set of nodecompensating angles for the relationship between desired j-th groundcontact portion position/posture and the height and inclination of asupposed j-th floor surface (specifically, the height and inclination ofa supposed floor surface at a supposed j-th floor contact point) and therelationship between corrected desired j-th ground contact portionposition/posture that has been corrected by a compliance operation usinga set of node compensating angles and the height and inclination of anactual j-th floor surface (specifically, the height and inclination ofan actual floor surface at an actual j-th floor contact point) to agreewith each other is referred to as “(set of) node floor inclinationerrors),” and among these, the component of the node floor inclinationerror corresponding to an n-th node compensating angle is referred to as“n-th node floor inclination error.” If an n-th node is a leaf node,then the “n-th node floor inclination error” thus defined agrees with“the n-th node floor inclination error”(=the difference between anactual n-th ground contact portion floor inclination and a supposed n-thground contact portion floor inclination) defined in relation to leafnodes previously described.

After all, the set of node floor inclination errors corresponds to acompensation amount necessary to shift all ground contact portions of arobot that is traveling according to a desired gait from a state inwhich they are parallel to a desired floor surface to a state in whichthey are parallel to an actual floor surface.

Therefore, by estimating a floor configuration error while walking andby adding the estimated floor configuration error to desired groundcontact portion position/posture, the actual floor reaction force momentof each node will be the same as that when the robot is walking on asupposed floor even if there is a floor configuration error. Naturally,the actual floor reaction force moment of the ground contact portion,which is the actual floor reaction force moment of a leaf node, agreeswith a desired ground contact portion floor reaction force moment.

Normally, a floor configuration may be represented using the “n-th nodefloor inclination error” defined as above. If, however, a certain nodehas three child nodes, and the desired floor reaction force centralpoints of the three child nodes are aligned on the same straight line orif a certain node has four or more child nodes, then it would bedifficult to represent a floor configuration.

Hence, in the explanation given below, the following expressions thatcan be generally applied will be used for n-th nodes that are notleaves. The weights used to determine a predetermined weighted averagein the following definition are to be the same as the aforesaid weightsdetermined as described above by the desired floor reaction forcedistributor 102.

Definition:

When all the heights and inclinations of a desired ground contactportion are set to agree with the corresponding heights and inclinationsof an actual floor surface, and when the desired floor reaction forcecentral point of an arbitrary node is expressed by a predeterminedweighted average of the desired floor reaction force central points ofall child nodes thereof (i.e., an internally dividing point based on apredetermined internal division ratio), regarding an arbitrary n-thnode, the height obtained by subtracting the height (the position in thevertical direction) of the desired floor reaction force central point ofa parent node of the n-th node from the height (the position in thevertical direction) of the desired floor reaction force central point ofthe n-th node is referred to as “actual n-th node relative floorheight.”Definition:When all the heights and inclinations of a desired ground contactportion are set to agree with the corresponding heights and inclinationsof a supposed floor surface, and when the desired floor reaction forcecentral point of an arbitrary node is expressed by a predeterminedweighted average of the desired floor reaction force central points ofall child nodes thereof (i.e., an internally dividing point based on apredetermined internal division ratio), regarding an arbitrary n-thnode, the height obtained by subtracting the height (the position in thevertical direction) of the desired floor reaction force central point ofa parent node of the n-th node from the height (the position in thevertical direction) of the desired floor reaction force central point ofthe n-th node is referred to as “supposed n-th node relative floorheight.”

The height obtained by subtracting a supposed n-th node relative floorheight from an actual n-th node relative floor height is referred to asan “n-th node relative floor height error.” The n-th node relative floorheight error will take the same value when defined as follows.

Definition:

When all the heights and postures of desired ground contact portions aremade to agree with corresponding ground contact portion floor heighterrors and ground contact portion floor inclination errors, and thedesired floor reaction force central point of an arbitrary node isexpressed by a predetermined weighted average of the desired floorreaction force central points of all child nodes thereof (i.e., aninternally dividing point based on a predetermined internal divisionratio), regarding an arbitrary n-th node, the height obtained bysubtracting the desired floor reaction force central point of a parentnode of the n-th node from the desired floor reaction force centralpoint height of the n-th node is referred to as “n-th node relativefloor height error.”

As is obvious from the definition of the n-th node relative floor heighterror described above, regarding each node having child nodes, an n-thnode relative floor height error of all child nodes thereof representsthe relative relationship of floor height errors. Further, regardingeach node having child nodes, the weighted average value of all childnodes thereof is zero.

In the present embodiment (the third embodiment), the difference betweenan actual floor surface and a supposed floor surface on a desired gait(i.e., a floor configuration error) is compensated for so as to correctthe desired positions/postures of ground contact portions 10 to make theground contact portions 10 that are to be in contact with the groundproperly come in contact with the actual floor surface. Further, in thepresent embodiment, the n-th node relative floor height error defined asdescribed above is used as a floor configuration parameter representinga floor configuration error, and based on this, each n-th ground contactportion floor height error (floor configuration error) is estimated.Then, based on each estimated n-th ground contact portion floor heighterror (hereinafter referred to simply as the n-th floor height error insome cases), the desired position of the n-th ground contact portion 10is corrected.

Based on the above premises, essential sections of the presentembodiment will be explained in detail.

The inputs into the floor configuration estimator 130 of thehierarchical compliance operation determiner 114 shown in FIG. 35 are,in general, roughly divided as follows.

-   1) Desired floor reaction force central points (desired total floor    reaction force central points P or desired n-th ground contact    portion floor reaction force central points Qn (n=1, 2, 3, 4, 5,    6)).-   2) Supposed floor surface configuration (coordinates of supposed    n-th floor contact points and supposed n-th ground contact portion    floor inclinations).-   3) Final desired postures to be followed by an actual robot    (corrected desired ground contact portion positions/postures with    deformation compensation) or actual joint displacements or frequency    weighted average thereof (weighted average based on a weight having    a frequency characteristic).-   4) Body posture inclination errors.-   5) Actual floor reaction forces (the translational force component    and the moment component of an actual n-th ground contact portion    floor reaction force).

Incidentally, in the robot 1 in the present embodiment, the postures ofthe ground contact portions 10 cannot be controlled, so that thesupposed n-th ground contact portion inclination of a supposed floorsurface configuration is unnecessary.

Further, in the robot 1 in the present embodiment, the moment componentof an actual n-th ground contact portion floor reaction force of anactual floor reaction force is zero. This, therefore, is not required tobe supplied to the floor configuration estimator 130. However, in theexplanation of the present embodiment hereinafter, the n-th groundcontact portion floor reaction force generally includes the momentcomponent of an actual n-th ground contact portion floor reaction forcealso, considering a case where the postures of the ground contactportions 10 are controllable. Further, in the following explanation, thetotal number of ground contact portions will be frequently referred toas “last leaf node number” so as to make it possible to expandably applythe present embodiment easily also to a case where the number of groundcontact portions is other than four or six. For example, in thesix-legged robot 1 shown in FIG. 1, the last leaf node number is “6”.

FIG. 40 is a block diagram showing the processing functions of the floorconfiguration estimator 130. The functional elements constituting thefloor configuration estimator 130 shown in FIG. 40 will be explained.The floor configuration estimator 130 is equipped with a mechanismcompliance model 134.

The mechanism compliance model 134 determines the deformation amount ofa compliance mechanism 42 or the like of each of the legs #1 to #6 (thecompliance mechanism 42 and a link mechanism of each leg) when each ofthe ground contact portions 10 is subjected to an actual floor reactionforce Fn_act (n=1, 2, . . . , last leaf node number), and adds thedetermined deformation amount to the corrected desired ground contactportion position/posture with deformation compensation, which aresupplied to the aforesaid robot geometric model 110 (refer to FIG. 2),so as to determine estimated n-th ground contact portionposition/posture (n=1, 2, . . . , last leaf node number), which are theestimated values of the position/posture of an n-th ground contactportion (each ground contact portion) after mechanism deformation (morespecifically, the estimated values of the n-th ground contact portionposition/posture when it is assumed that an actual body posture agreeswith a desired body posture). Incidentally, the corrected desired groundcontact portion position/posture with deformation compensation used inthis case are the past values of the values or the like determined inthe last control cycle.

When determining the estimated ground contact portion position/postureafter mechanism deformation, if a follow-up delay of control of a jointdisplacement of the robot 1 cannot be ignored, then the position/postureobtained by passing corrected desired ground contact portionposition/posture with deformation compensation through a low-pass filtercorresponding to the follow-up delay may be used instead of using thecorrected desired ground contact portion position/posture withdeformation compensation as it is.

Alternatively, from a detected value of an actual joint displacement ofthe robot 1, the actual ground contact portion position/posture withoutmechanism deformation, which are the actual ground contact portionposition/posture in a case where there is no mechanism deformation, aredetermined through a robot geometric model (the same model as the robotgeometric model 110 in FIG. 2), and the determined values may be used inplace of the corrected desired ground contact portion position/posturewith deformation compensation.

Alternatively, a weighted average of the aforesaid actual ground contactportion position/posture without mechanism deformation and the correcteddesired ground contact portion position/posture with deformationcompensation may be determined by using a frequency weight (weighthaving a frequency characteristic), and this may be used in place of thecorrected desired ground contact portion position/posture withdeformation compensation.

Strictly speaking, the actual ground contact portion position/posturewithout mechanism deformation mentioned above should be used; however,any one of the above may be used without much difference as long as thecapability of joint displacement control is high.

Incidentally, the mechanism compliance model 134 is explained in detailin Japanese Unexamined Patent Application Publication No. H10-277969previously proposed by the present applicant; therefore, no moreexplanation will be given herein.

Supplementally, in a case where an input to the robot geometric model110 in FIG. 2 does not include deformation compensation, that is, ifcorrected desired ground contact portion position/posture withoutdeformation compensation (the position/posture determined by theaforesaid corrected desired ground contact portion position/posturecalculator 114 g) are supplied to the robot geometric model 110, thenthe corrected desired ground contact portion position/posture withoutdeformation compensation may be supplied to the mechanism compliancemodel 134 of the floor configuration estimator 130.

Returning to the explanation of FIG. 40, the estimated ground contactportion position/posture after mechanism deformation (n=1, 2, . . . , 6)are then rotationally moved by the aforesaid body posture inclinationerror θberr, the desired total floor reaction force central point Pbeing the center of the rotation, thereby to determine n-th estimatedground contact portion position/posture after mechanism deformation(n=1, 2, . . . , last leaf node number) observed from a globalcoordinate system (supporting leg coordinate system). Subsequently, theposition of the point corresponding to the desired n-th ground contactportion floor reaction force central point after the mechanismdeformation observed from the global coordinate system is determined onthe basis of the n-th estimated ground contact portion position/postureafter the mechanism deformation (n=1, 2, . . . , last leaf node number)observed from the global coordinate system and the position of thedesired n-th ground contact portion floor reaction force central pointobserved from the desired ground contact portion position of an n-thground contact portion (the desired position of the representative pointof the n-th ground contact portion). Then, the determined position ofthe point is defined as an instantaneous estimated floor contact pointposition (instantaneous estimated n-th floor contact point position)Qn_estm′. This Qn_estm′ corresponds to the estimated value of theinstantaneous position of an actual n-th floor contact point. The methodfor determining the instantaneous estimated floor contact point positionQn_estm′ is equivalent to the method for determining Q″ of expression 21in Japanese Unexamined Patent Application Publication No. H10-277969, sothat no further detailed explanation thereof will be given herein.

Subsequently, the result obtained by subtracting the supposed n-th floorcontact point position Dn from the instantaneous estimated n-th floorcontact point position Qn_estm′ provides a bias-included instantaneousn-th ground contact portion floor height error. This Zfn_with_bias′corresponds to the instantaneous estimated value of an n-th groundcontact portion floor height error. In a case where no actual height ofa body is detected, as with the present embodiment, a bias error isusually included. Hence, Zfn_with_bias′ is referred to as thebias-included instantaneous n-th ground contact portion floor heighterror.

Here, as previously defined, the supposed n-th floor contact point Dnlies at the same position as a desired n-th ground contact portion floorreaction force central point Qn in a period during which an n-th groundcontact portion is in contact with the ground. In a desired gait,immediately before an n-th ground contact portion comes in contact withthe ground, the position of the desired n-th ground contact portionfloor reaction force central point Qn that is expected at the nextground contact is defined as the supposed n-th floor contact point Dn.In the desired gait, immediately after the n-th ground contact portionleaves a floor, the position of the desired n-th ground contact portionfloor reaction force central point Qn that was expected at the time ofleaving the floor is defined as the supposed n-th floor contact pointDn.

Subsequently, an estimated n-th node floor height error Zfn_estm (n=1,2, . . . , last leaf node number) is determined by the floor heighterror estimation processing subroutine of the flowchart of FIG. 41 onthe basis of the bias-included instantaneous n-th ground contact portionfloor height error Zfn_with_bias′ (n=1, 2, . . . , last leaf nodenumber) and the n-th ground contact portion floor reaction force Fn_act(n=1, 2, . . . , last leaf node number).

Here, “hierarchical relativization processing” (or “hierarchicalrelativization”) used in the floor height error estimation processingsubroutine or the like will be explained.

The hierarchical relativization processing is generally defined as theprocessing for determining the output values of all nodes relative tothe sets of values (the values of predetermined types of state amounts)input to all leaf nodes. More specifically, the hierarchicalrelativization processing is the processing for determining node outputvalues such that the weighted average of output values corresponding toall childe nodes of an arbitrary node that is not a leaf node is zeroand the input value (state amount) of an arbitrary leaf node agrees withthe sum of the output value of the node and the output values of allancestor nodes of the node.

Hereinafter, determining the value of a certain type of output BB by thehierarchical relativization processing from a certain type of input(state amount) AA to a leaf node will be referred to as “determining BBby hierarchically relativizing AA.”

The following will explain the algorithm of the hierarchicalrelativization processing. In general, an input of the hierarchicalrelativization processing is an n-th ground contact portion height Zfn(ndenoting a leaf node number) and an output thereof is an n-th noderelative height Zn_re1 (n=1, 2, . . . , last node number). Incidentally,the “last node number” means the largest number among all node numbers,and the last node number is, for example, 145236 in the hierarchicalstructure explained in the second embodiment. Further, in theexplanation of the algorithm, the “n-th ground contact portion height”is a designation that generically represents an input of thehierarchical relativization processing, such as the aforesaid n-thground contact portion floor height or an n-th ground contact portionfloor height error, and the “n-th node relative height” is a designationthat generically represents an output of the hierarchical relativizationprocessing, such as an n-th ground contact portion relative floor heighterror, which will be discussed later.

First, a bias-included n-th node height Zn_with_bias is determined asfollows. If an n-th node is a leaf node (i.e., the ground contactportion), then the value of an n-th ground contact portion height Zfn issubstituted into the bias-included n-th node height Zn_with_bias. If then-th node is not a leaf node (i.e., the ground contact portion), thenthe weighted average of the bias-included n-th node heights Zn_with_biasof all child nodes of the n-th node is determined. The determinedweighted average is defined as the bias-included n-th node heightZn_with_bias. It should be noted that a weight Wj (j=1, 2, . . . )determined by the aforesaid desired floor reaction force distributor 102is used as the weight for each child node.

More specifically, the bias-included n-th node height Zn_with_bias isobtained by expression 32 given below.

If the n-th node is a leaf node;Zn_with_bias=ZfnIf the n-th node is not a leaf node;Zn_with_bias=Σ(Zj_with_bias*Wj)  Expression 32

where Σ denotes the total sum on j, which is j∈{set of child nodenumbers of the n-th node}

According to the above rules, the bias-included node height Zn_with_bias(n=1, 2, . . . , last node number) is determined on every node.

Lastly, an n-th node relative height Zn_re1 is determined by subtractingthe bias-included height Zh_with_bias of a parent node of the n-th node(this is supposed to be an h-th node) from the bias-included n-th nodeheight Zn_with_bias.

More specifically, the n-th node relative height Zn_re1 is determinedaccording to the following expression 33. It should be noted that theh-th node is the parent node of the n-th node.Zn _(—) re1=Zn_with_bias−Zh_with_bias  Expression 33

According to the above rule, the node relative floor height Zn_re1(ndenoting the number of each node) is determined on every node. It shouldbe noted that a node relative height Zk_rel(k denoting the number of aroot node) with respect to a root node is zero. FIG. 42 shows an exampleof calculation of a node relative height of a four-legged robot shown inthe first embodiment, while FIG. 43 shows an example of calculation of anode relative height of the six-legged robot in the present embodiment.

The bias-included n-th node height Zn_rel_with_bias may alternatively bedetermined by the following method, which provides the same results.

If an n-th node is a leaf node (i.e., the ground contact portion), thenthe value of an n-th ground contact portion height Zfn is substitutedinto the bias-included n-th node height Zn_with_bias. If the n-th nodeis not a leaf node (i.e., the ground contact portion), then the weightedaverage of the bias-included heights of the leaf nodes of alldescendants of the n-th node is determined. The determined weightedaverage is defined as the bias-included n-th node height Zn_with_bias.It should be noted that a weight Wj′ is the product of the weight Wjdetermined for the node j by the aforesaid desired floor reaction forcedistributor 102 and the weight determined by the aforesaid desired floorreaction force distributor 102 for an ancestor node of the node j andall nodes, which are the descendant nodes of the n-th node.

More specifically, the bias-included n-th node height Zn_with_bias isobtained by expression 34 given below.

If the n-th node is a leaf node;Zn_with_bias=ZfnIf the n-th node is not a leaf node;Zn_with_bias=Σ(Zj_with_bias*Wj′)  Expression 34

where Σ denotes the total sum on j, which is j∈{set of leaf node numbersof the descendants of a node n}

Supplementally, the weight Wj′ in this case agrees with the valueobtained by dividing the desired floor reaction force of each leaf nodeby a desired j-th node floor reaction force if the desired j-th nodefloor reaction force is not zero.

The above is the hierarchical relativization processing.

The subroutine processing of the estimation processing of a floor heighterror will now be explained below by mainly using the flowchart of FIG.41 showing it. Incidentally, this processing uses an n-th ground contactportion floor height error as an input of the hierarchicalrelativization processing, and an n-th node relative floor height erroras an output. The like symbols as those shown in the aforesaidexpressions 32 to 34 or the like will be used as the reference symbolsof these input and output.

Referring to FIG. 41, first, in S50, the aforesaid bias-includedinstantaneous n-th ground contact portion floor height errorZfn_with_bias′ is hierarchically relativized to determine aninstantaneous n-th node relative floor height error Zn_rel′ (n=1, 2, . .. , last node number).

Subsequently, in S52, the output obtained by hierarchically relativizingan estimated n-th ground contact portion floor height error, which is anestimated value of the n-th ground contact portion floor height errordetermined at the last control cycle of the control device 50(hereinafter referred to as the last estimated n-th ground contactportion floor height error Zfn_estm_p), is determined as the lastestimated n-th node relative floor height error Zn_rel_estm_p(n=1, 2, .. . , last node number). It should be noted that, as the weight of eachnode used in this hierarchical relativization processing, the weight Wn(n=1, 2, . . . last node number) determined by the aforesaid desiredfloor reaction force distributor 102 at the current (present) controlcycle of the control device 50 is used.

Supplementally, in general, if the weight determined in the currentcontrol cycle (hereinafter referred to as the current weight) and theweight determined in the last control cycle (hereinafter referred to asthe last weight) are different, then the value of the last estimatedrelative floor height error of each node determined using the lastweight and the value of the last estimated relative floor height errorof each node determined using the current weight will be inconvenientlydifferent values even if the configuration of an actual floor does notchange. Hence, the last estimated node relative floor height error ofeach node is calculated anew by using the current weight as describedabove instead of directly using the estimated relative floor heighterror of each node determined at the last control cycle as the lastestimated node relative floor height error of each node.

Subsequently, in S54, an n-th node relative floor height errorcorrection amount candidate value Zn_inc_cand (n=1, 2, . . . , last nodenumber) is determined according to expression 35 given below. TheZn_inc_cand means the provisional value of the correction amount of theerror when the n-th node relative floor height error is updated at thecurrent control cycle.

In this case, the n-th node relative floor height error correctionamount candidate value Zn_inc_cand is set to the product of thedifference between the instantaneous n-th node relative floor heighterror Zn_rel′ and the last estimated n-th node relative floor heighterror Zn_rel_estm_p and a predetermined coefficient (ΔT/(Testm+ΔT)), asshown by expression 35 given below.Zn_inc_cand=(Zn_rel′−Zn_rel_estm_(—) p)*ΔT/(Testm+ΔT)  Expression 35

where Testm in expression 35 denotes a time constant (the time constantof a primary delay) at the estimation (update) of an n-th node floorheight error, and ΔT denotes a control cycle of the control device 50.

Thus, when Zn_inc_cand is set, Zn_inc_cand is added to Zn_rel_estm_p foreach control cycle of the control device 50, causing the value of theresult of the addition to change so as to gradually approximate Zn_rel′.

Subsequently, in S56, a node request mode (an n-th request modemdn_dmd), which is a required value of the mode of each node, isdetermined on the basis of a timing of a desired gait.

Specifically, in the aforesaid six-legged robot 1, an n-th node requestmode (n=1, 2, 3, 4, 5, 6), a 145th node request mode md145 dmd, a 236thnode request mode md236 dmd, and a 145236th node request mode md145236dmd are set as shown in the timing chart of FIG. 44 with respect to theelapse of time of a desired gait, and a current request mode isdetermined on the basis thereof. However, if the gait is different, thenthe pattern of a node request mode should be changed accordingly.Incidentally, regarding ON/OFF in the timing charts of a first stage(the uppermost stage) and a second stage in FIG. 44, the state in whichthe ground contact portion is in contact with the ground corresponds toON, while the state in which it is not in contact with the groundcorresponds to OFF.

Supplementally, as the present applicant has disclosed in JapaneseUnexamined Patent Application Publication No. H10-277969, in the case ofa robot that has feet as ground contact portions and that is capable ofestimating floor inclination errors in the ground contact portions, therequest mode corresponding to the floor inclination error estimation ofthe ground contact portions should be set as explained in thePublication.

As shown in FIG. 44, the mode of each node (hereinafter referred tosimply as the node mode) comes in ready mode, hold mode, and reset mode.

The ready mode is the mode for estimating a floor configuration. Thehold mode is the mode for holding the estimated value of the floorconfiguration (an estimated n-th ground contact portion floor heighterror)(for maintaining an immediately preceding value). The hold mode ismade to exist in a period during which the estimated value of the floorconfiguration may diverge and a period during which the accuracy of theestimated value of the floor configuration may deteriorate. Further, thereset mode is the mode for shifting the estimated value of the floorconfiguration to a predetermined initial value before the next floorconfiguration estimation is started (the next ready mode begins).

Regarding an n-th node (n=1, 2, 3, 4, 5, 6), which is a leaf node, if ann-th ground contact portion is in contact with the ground on a desiredgait (that is, if a desired n-th ground contact portion floor reactionforce is not zero), then the n-th node request mode is set to the readymode. Thereafter, when the n-th ground contact portion leaves a floor onthe desired gait (that is, when the desired n-th ground contact portionfloor reaction force becomes zero), the n-th node request mode is set tothe hold mode immediately thereafter. Then, after a while, the n-th noderequest mode is set to the reset mode. Further, immediately before then-th ground contact portion comes in contact with the ground on thedesired gait, the n-th node request mode is set to the ready mode.

Regarding an n-th node (n=145, 236, 145236), which is not a leaf node,that is, an n-th node having child nodes, if at least one ground contactportion belonging to the n-th node is in contact with the ground on thedesired gait, then the n-th node request mode is set to the ready mode.Thereafter, when all ground contact portions belonging to the n-th nodeleave the floor on the desired gait, then the n-th node request mode isset to hold until immediately thereafter. Alternatively, the n-th noderequest mode may be set to the hold mode from immediately before all theground contact portions belonging to the n-th node leave the floor.Then, after a while, the n-th node request mode is set to the resetmode. Further, immediately before at least one of the ground contactportions belonging to the n-th node comes in contact with the ground onthe desired gait, the n-th node request mode is set to the ready mode.

Subsequently, in S58, it is determined whether an estimation enablecondition is satisfied, and the mode of each node is finally determinedon the basis of the result of the determination and the node requestmode. The mode of each node to be determined is one of the aforesaidready mode, hold mode, and reset mode. The aforesaid node request modehas been determined on the basis of whether the ground contact portionsare in contact with the ground or not on the desired gait. In S58, themode of each node is determined, considering whether the ground contactportions are actually in contact with the ground or not.

The estimation enable condition means satisfying either expression 36 or37 given below. In this case, if neither expression 36 or 37 issatisfied, then the estimation enable condition does not hold.Fn_act_z>Fn_min  Expression 36Zn_rel′<Zn_rel_estm_p  Expression 37

where Fn_act_z denotes the translational force vertical component of anactual n-th ground contact portion floor reaction force (n=1, 2, . . . ,last leaf node number).

The estimation enable condition is a condition that when an n-th noderelative floor height error is estimated (when a floor configurationerror is estimated), the estimated value does not diverge. Thedivergence here means a situation in which, when a correcting operationis performed to modify corrected desired ground contact portionposition/posture so as to offset the influence of an actual n-th floorheight error by using an estimated n-th floor height error Zfn_estm (theestimated value of the floor configuration error) that has beenestimated as it will be discussed later, the estimated n-th floor heighterror Zfn_estm continues to increase and an n-th ground contact portionmoves away from the floor (floats further from the floor).

If the estimation of Zfn_estm is ideally executed, a predeterminedpermissible value Fn_min may be zero, whereas in actuality, a detectionerror of Fn_act takes place, causing an estimated value to diverge insome cases. To prevent it, Fn_min is set to be a value that issufficiently larger than a detection error of Fn_act.

In the present embodiment, the estimation enable condition is thecondition in which divergence described above does not occur. However,the estimation enable condition may alternatively be a condition inwhich a predetermined accuracy of estimating a floor configuration canbe secured. Hence, in place of the translational force verticalcomponent of the actual n-th ground contact portion floor reaction forceFn_act, a component that is perpendicular to a supposed floor surface(or an estimated floor surface (estimated actual floor surface)) out ofthe translational force components of the actual n-th ground contactportion floor reaction force Fn_act may be used.

Incidentally, if a floor configuration error (a configuration errorbetween an actual floor surface and a supposed floor surface) is simplyestimated and not reflected in a walking operation, that is, if thecorrection of the corrected desired ground contact portionposition/posture to cancel the influence of a floor configuration erroris not carried out by using an estimated value of the floorconfiguration error, then the estimated value of the floor configurationerror does not diverge.

The mode of each node is determined on the basis of the result of thedetermination on whether the estimation enable condition is satisfiedand on the basis of a node request mode.

If an n-th node request mode is the ready mode and the estimation enablecondition is satisfied, then the n-th node mode is set to the readymode.

If an n-th node request mode is the ready mode and the estimation enablecondition is not satisfied, then the n-th node mode is set to the holdmode.

If an n-th node request mode is the hold mode, then the n-th node modeis set to the hold mode.

If an n-th node request mode is the reset mode, then the n-th node modeis set to the reset mode.

Thus, an n-th mode is determined to be the ready mode at the end only ifthe request mode is the ready mode and the estimation enable conditionis satisfied.

Subsequently, in S60 to S70, the n-th node relative floor height error(n=1, 2, . . . , last node number) associated with each of all nodes isestimated.

Supplementally, as disclosed in Japanese Unexamined Patent ApplicationPublication No. H10-277969, in the case of a robot that has groundcontact portions (feet) whose postures can be controlled and that iscapable of estimating the floor inclination errors in the ground contactportions, it is preferred to also estimate the floor inclination errorsassociated with the ground contact portions.

Therefore, the processing of S60 to S70 is configured so as to allowfloor configuration errors to be estimated also when the postures ofground contact portions can be controlled.

The following will specifically explain the processing of S60 to S70. InS62, the quantity of child nodes of an n-th node is determined. At thistime, if the number of child nodes (the quantity of child nodes) is two,then in S64, the floor configuration estimation processing for two childnodes, which is the estimation processing of the floor configurationerror for the quantity, is carried out. If the number of child nodes isthree, then in S66, the floor configuration estimation processing forthree child nodes, which is the estimation processing of the floorconfiguration error for the quantity, is carried out. If the number ofchild nodes is zero, then it is determined in S68 whether the n-th nodeground contact portion permits control of a floor reaction force moment.

The processing for the case where the number of child nodes is zero willbe further explained. If the number of child nodes of an n-th node iszero, then it means that the n-th node is a leaf node, and the n-th nodecorresponds to the ground contact portion. In this case, as describedabove, first, it is determined in S68 whether the n-th ground contactportion permits control of a floor reaction force moment. Here, forexample, in the case of a robot that has feet whose postures can becontrolled as ground contact portions and its legs do not include freejoints, as with a regular bipedal walking robot, floor reaction forcemoments can be generated at the ground contact portions. However, in therobot 1 (six-legged robot) in the present embodiment, the ground contactportions 10 engage the spherical joints 12, which are free joints;therefore, none of the ground contact portions 10 can generate floorreaction force moments. In this case, no processing is carried out onthe n-th node whose number of child nodes is zero.

Meanwhile, if it is determined in S68 that floor reaction force momentscan be controlled, then ground contact portion floor inclination errorestimation processing is carried out in S70. This processing is theprocessing for estimating the floor inclination error in each groundcontact portion. This processing is the processing in which the footfloor reaction force central point in the floor inclination estimationprocessing in Japanese Unexamined Patent Application Publication No.H10-277969 previously proposed by the present applicant has beenreplaced by the desired floor reaction force central point of an n-thground contact portion in the present embodiment. Hence, furtherdetailed explanation will be omitted in the present description.

Next, the aforesaid floor configuration estimation processing for atwo-children node will be explained with reference to FIG. 45 to FIG.48. FIG. 45 is a flowchart showing the subroutine processing of thefloor configuration estimation processing for a two-children node, FIG.46 and FIG. 47 are flowcharts showing the subroutine processing of S6404and S6408, respectively, of FIG. 45, and FIG. 48 is a flowchart showingthe subroutine processing of S6406 and S6410 of FIG. 45.

In the floor configuration estimation processing for two-children nodes,the modes of all child nodes of an n-th node having two child nodes (themode determined in S58 of FIG. 41) are determined in S6400, as shown bythe flowchart of FIG. 45. The results of the determination are dividedinto three, namely, “all ready,” “all reset,” and “others (else).” Inthe following explanation, the two child nodes of the n-th node will bedenoted as the i-th node and the j-th node.

Here, if it is determined that the modes of the child nodes are “allready,” then it is determined in S6402 whether a resultant forceFn_z(=Fi_act_z+Fj_act_z) of the translational force vertical componentsFi_act_z, Fj_act_z of the actual node floor reaction forces of the childnodes of the n-th node is larger than a predetermined value Fn_min2. Inother words, Fn_z denotes the translational force vertical component ofthe resultant force of the actual floor reaction forces of all groundcontact portions belonging to the n-th node.

If the determination result of S6402 is YES, then intra-group completeestimation processing for a node having two child nodes (the processingfor virtually estimating the node relative floor height error of each ofthe two child nodes owned by the n-th node) is carried out in S6404. Inthis processing, estimated node relative floor height errors Zi_rel_estmand Zj_rel_estm of the i-th node and the j-th node, respectively, whichare the two child nodes of the n-th node, are determined (updated)according to the expressions shown in the flowchart of FIG. 46. Morespecifically, Zi_rel_estm will be representatively explained. An i-thnode relative floor height error correction amount candidate valueZi_inc_cand determined in the aforesaid S54 is added to a valueZi_rel_estm_p of Zi_rel_estm in the last control cycle thereby todetermine a new estimated i-th node relative floor height errorZi_rel_estm. The same applies to the j-th node.

Further, if the resultant force Fn_z of the floor reaction forces of thetwo child nodes of the n-th node is smaller than the predetermined valueFn_min2 (if the determination result of S6402 is NO), then it means thatthe accuracy of estimating a floor configuration error would beexcessively deteriorated, so that no substantial estimation processingis carried out, and the intra-group total hold processing for a nodehaving two child nodes (the processing for holding the estimated noderelative floor height errors of the two child nodes of the n-th nodewithout updating them) is carried out in S6406. In the hold processing,as shown by the expressions in the flowchart of FIG. 48, the values ofthe estimated node relative floor height errors Zi_rel_estm andZj_rel_estm of the i-th node and the j-th node, respectively, aremaintained at the values Zi_rel_estm_p and Zj_rel_estm_p in the lastcontrol cycle.

Next, if it is determined in S6400 of FIG. 45 that the modes of the twochild nodes are “all reset,” then intra-group total reset processing fora node having two child nodes (the processing for resetting theestimated node relative floor height error of each of the two childnodes owned by the n-th node) is carried out in S6408. In the resetprocessing, the estimated node relative floor height errors Zi_rel_estmand Zj_rel_estm of the i-th node and the j-th node, respectively, areupdated according to the expressions in the flowchart of FIG. 47 suchthat they gradually approximate zero. Incidentally, the meanings of ΔTand Testm in the expressions are the same as those of the aforesaidexpression 35.

More generally speaking, the reset processing is the processing in whichZi_rel_estm and Zj_rel_estm are determined to take values that arecloser to zero than Zi_rel_estm_p and Zj_rel_estm_p are, whilesatisfying a condition in which the weighted average value ofZi_rel_estm and Zj_rel_estm is zero, that is,Wi*Zi_rel_estm+Wj*Zj_rel_estm=0. Incidentally, the reset processingincludes a primary delay element, so that infinite time is required forthe node relative floor height errors to completely return to zero.Hence, a finite stabilization function generator previously proposed bythe present applicant (Japanese Unexamined Patent ApplicationPublication No. H5-324115) may be used to gradually reset the noderelative floor height errors to zero.

Further, if the modes of the two child nodes are “others,” that is, ifthe modes are neither “all ready” nor “all reset” in S6400 of FIG. 45(for example, if the modes of the two child nodes are all hold modes),then the same processing as that of S6406 (refer to FIG. 48) is carriedout in S6410.

As described above, only in a case where the modes of the two childnodes of the n-th node are all ready, and there is no danger in that thedivergence takes place in the processing of estimating node relativefloor height errors, the substantial estimation processing of the noderelative floor height errors of the two child nodes is carried out. And,even if the modes of the child nodes are all ready, if there is a dangerof the occurrence of divergence of estimation processing, then thevalues of the estimated node relative floor height errors of the twochild nodes are hold. Further, in a situation wherein the actual nodefloor reaction forces of the two child nodes are both zero, that is, ifno actual floor reaction forces act on any ground contact portions 10belonging to an n-th node, then the node relative floor height errors ofthe two child nodes are reset so that they are gradually reset to zero.

Referring now to FIG. 49 to FIG. 55, the floor configuration estimationprocessing for a three-children node S66 in FIG. 41 will be explained.FIG. 49 is a flowchart showing the subroutine processing of thethree-children node floor configuration estimation processing, FIG. 50and FIG. 51 are flowcharts showing the subroutine processing of S6604and S6608 of FIG. 49, FIG. 52 is a flowchart showing the subroutineprocessing of S6606, S6614, and S6618 of FIG. 49, and FIG. 53 and FIG.54 are flowcharts showing the subroutine processing of S6612 and S6616,respectively, of FIG. 49.

In the three-children node floor configuration estimation processing, asshown in the flowchart of FIG. 49, the modes of all child nodes of ann-th node having three child nodes (the modes determined in S58 of FIG.41) are determined in S6600. The determination results are divided intosix, namely, “all ready,” “all reset,” “only two child nodes are ready,”“only one child node is hold, and the rest are reset,” “only two childnodes are hold and the rest is reset,” and “others (else).”

Here, in the flowchart of FIG. 49, to explain more generally the floorconfiguration estimation processing for three child nodes,considerations are given not only to a case where all the ground contactportions 10 belonging to each intermediate node (the 145th node and the236th node) simultaneously come in contact with the ground or leave afloor, as explained in the aforesaid second embodiment, but also to acase where a desired gait of a robot is generated such that one of theground contact portions of each intermediate node is in contact with theground, while the remaining ground contact portions leave the floor. Inthis case, unlike the two-children node type, it is necessary to alsoconsider a case where the modes of the three child nodes are “only twochild nodes are ready,” “only one child node is hold, and the remainingones are reset,” or “only two child nodes are hold, and the remainingare reset.” In the following explanation, the three child nodes of ann-th node will be referred to as an i-th node, a j-th node, and a k-thnode.

Here, if it is determined that the modes of child nodes are “all ready,”then it is determined in S6602 whether a resultant forceFn_z(=Fi_act_z+Fj_act_z+Fk_act_z) of the translational force verticalcomponents Fi_act_z, Fj_act_z, and Fk_act_z of the actual node floorreaction forces of the child nodes of the n-th node is larger than apredetermined value Fn_min2. In other words, Fn_z denotes thetranslational force vertical component of the resultant force of theactual floor reaction forces of all ground contact portions belonging tothe n-th node.

If the determination result of S6602 is YES, then intra-group completeestimation processing for a node having three child nodes (theprocessing for virtually estimating the node relative floor height errorof each of the three child nodes owned by the n-th node) is carried outin S6604. In this processing, estimated node relative floor heighterrors Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm of the i-th node, j-thnode, and the k-th node, respectively, which are the three child nodesof the n-th node, are determined (updated) according to the expressionsshown in the flowchart of FIG. 50. More specifically, Zi_rel_estm willbe representatively explained. An i-th node relative floor height errorcorrection amount candidate value Zi_inc_cand determined in theaforesaid S54 is added to a value Zi_rel_estm_p of Zi_rel_estm in thelast control cycle thereby to determine a new estimated i-th noderelative floor height error Zi_rel_estm. The same applies to the j-thnode and the k-th node.

If the determination result of S6602 is NO, then it means that theaccuracy of estimating a floor configuration error would be excessivelydeteriorated, so that no substantial estimation processing is carriedout, and the intra-group total hold processing for a node having threechild nodes (the processing for holding the estimated node relativefloor height errors of the three child nodes of the n-th node withoutupdating them) is carried out in S6606. In the hold processing, as shownby the expressions in the flowchart of FIG. 52, the values of theestimated node relative floor height errors Zi_rel_estm, Zj_rel_estm,and Zk_rel_estm of the i-th node, the j-th node, and the k-th node,respectively, are maintained at the values Zi_rel_estm_p, Zj_rel_estm_p,and Zk_rel_estm_p in the last control cycle.

Further, if it is determined in S6600 of FIG. 49 that the modes of thethree child nodes are “all reset,” then intra-group total resetprocessing for a node having three child nodes (the processing forresetting the estimated node relative floor height error of each of thethree child nodes owned by the n-th node) is carried out in S6608. Inthe reset processing, the estimated node relative floor height errorsZi_rel_estm, Zj_rel_estm, and Zk_rel_estm of the i-th node, the j-thnode, and the k-th node, respectively, are updated according to theexpressions in the flowchart of FIG. 51 such that they graduallyapproximate zero. Incidentally, the meanings of ΔT and Testm in theexpressions are the same as those of the aforesaid expression 35.

More generally speaking, the reset processing is the processing in whichZi_rel_estm, Zj_rel_estm, and Zk_rel_estm are determined to take valuesthat are closer to zero than Zi_rel_estm_p, Zj_rel_estm_p, andZk_rel_estm_p are, while satisfying a condition in which their weightedaverage value is zero, that is,Wi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0. Incidentally, theaforesaid finite stabilization function generator (Japanese UnexaminedPatent Application Publication No. H5-324115) may be used to resetZi_rel_estm_p, Zj_rel_estm_p, and Zk_rel_estm_p to zero.

Further, if it is determined in S6600 of FIG. 49 that the modes of thethree child nodes are “only two child nodes are ready,” then it isdetermined in S6610 whether the translational force vertical componentFn_z of the resultant force of the actual node floor reaction forces ofthe child nodes of an n-th node (the resultant force of the actual floorreaction forces of all ground contact portions 10 belonging to the n-thnode) is larger than the predetermined value Fn_min2.

If the result of the determination is YES, then the intra-group partialestimation processing for a node having three child nodes (theprocessing for substantially estimating the node relative floor heighterror of each of the three child nodes owned by the n-th node) iscarried out in S6612. This processing is carried out as shown by theflowchart of FIG. 53. In this case, it is assumed that the i-th node isnot in the ready mode, while the j-th node and the k-th node are in theready mode.

First, in S66120, the mode of the i-th node is determined. If the resultof the determination is the hold mode, then a new i-th node relativefloor height error correction amount candidate value Zi_inc_cand′ isdetermined to be zero in S66122, or if it is the reset mode, then thenew i-th node relative floor height error correction amount candidatevalue Zi_inc_cand′ is determined in S66124 according to the expressionin the figure. Zi_inc_cand′ determined in S66124 denotes an i-th noderelative floor height error correction amount candidate value forgradually bringing Zi_rel_estm_p to zero. The meanings of ΔT and Testmin the expressions of S66124 are the same as those of the aforesaidexpression 35. Supplementally, the aforesaid finite stabilizationfunction generator (Japanese Unexamined Patent Application PublicationNo. H5-324115) may be used to determine Zi_inc_cand′ in S66124.

Subsequently, in S66126, a new j-th node relative floor height errorcorrection amount candidate value Zj_inc_cand′ and a new k-th noderelative floor height error correction amount candidate valueZk_inc_cand′ are determined according to the expressions in the figure.

More specifically, Zj_inc_cand′ and Zk_inc_cand′ are determined suchthat they satisfy a condition represented byWi*Zi_inc_cand′+Wj*Zj_inc_cand′+Wk*Zk_inc_cand′=0 (a condition in whichthe weighted average value of Zi_inc_cand′, Zj_inc_cand′ andZk_inc_cand′ is zero) and a condition represented byZj_inc_cand′−Zj_inc_cand=Zk_inc_cand′−Zk_inc_cand.

Subsequently, in S66128, Zi_inc_cand′, Zj_inc_cand′, and Zk_inc_cand′determined as described above are added to the values Zi_rel_estm_p,Zj_rel_estm_p, and Zk_rel_estm_p of the relative floor height errors ofthe i-th node, the j-th node, and the k-th node in the last controlcycle so as to determine new Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm.

By determining Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm as describedabove, Zi_rel_estm, Zj_rel_estm, and Zk_rel_estm will be determined suchthat Zj_rel_estm-Zk_rel_estm approximates Zj_inc_cand-Zk_inc_cand whilesatisfying Wi*Zi_rel_estm+Wj*Zj_rel_estm+Wk+Zk_rel_estm=0 at the sametime.

If the result of the determination of S6610 of FIG. 49 is NO, then theaccuracy of estimating a floor configuration error would be excessivelydeteriorated, so that no substantial estimation processing is carriedout, but the same processing as that of the aforesaid S6606 (refer toFIG. 52) is carried out in S6614.

Further, if it is determined in S6600 of FIG. 49 that the modes of thethree child nodes are “only one child node is hold and the rest arereset,” then the processing for a case where only one child node is holdand the rest are reset is carried out in S6616. In the processing, newnode relative floor height errors Zi_rel_estm, Zj_rel_estm, andZk_rel_estm are determined according to the expressions shown in theflowchart of FIG. 54. Incidentally, in this case, it is assumed that themode of the i-th node is the hold mode and the modes of the j-th nodeand the k-th node are the reset mode. The meanings of ΔT and Testm inthe expressions are the same as those in the aforesaid expression 35.

The processing of FIG. 54 is, more generally, the processing fordetermining Zi_rel_estm to be zero, and determining Zj_rel_estm andZk_rel_estm to take values that are closer to zero than Zj_rel_estm_pand Zk_rel_estm_p are, while satisfyingWi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0 (a condition in which theweighted average value of Zi_rel_estm, Zj_rel_estm and Zk_rel_estm iszero). Supplementally, by the moment the modes of the j-th node and thek-th node both become the reset mode, their node weights Wj and Wkshould have become zero. Hence, based on a condition in that the sum ofthe weights Wi, Wj and Wk is 1, Wi will be 1 by that moment andWi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0, so that Zi_rel_estm willbe zero by that moment. Thus, holding the value of the i-th nodeZi_rel_estm means maintaining the value at zero.

Therefore, in the processing of FIG. 54, the value of Zi_rel_estm isdetermined to be zero.

Incidentally, the aforesaid finite stabilization function generator(Japanese Unexamined Patent Application Publication No. H5-324115) maybe used in the processing for gradually resetting Zj_rel_estm andZk_rel_estm to zero.

Further, if it is determined in S6600 of FIG. 49 that the modes of thechild nodes are “only two child nodes are hold and the rest is reset,”then the processing for a case where only two child nodes are hold andthe rest is reset is carried out in S6618. In the processing, new noderelative floor height errors Zi_rel_estm, Zj_rel_estm, and Zk_rel_estmare determined according to the expressions shown in the flowchart ofFIG. 55. Incidentally, in this case, it is assumed that the modes of thei-th node and the j-th node are both the hold mode and the mode of thek-th node is the reset mode. The meanings of ΔT and Testm in theexpressions are the same as those in the aforesaid expression 35.

The processing of FIG. 55 is, more generally, the processing for holdingZi_rel_estm and Zj_rel_estm at the values in the last control cycle, anddetermining Zk_rel_estm to take a value that is closer to zero thanZk_rel_estm_p is, while satisfyingWi*Zi_rel_estm+Wj*Zj_rel_estm+Wk*Zk_rel_estm=0 (a condition in which theweighted average value of Zi_rel_estm, Zj_rel_estm and Zk_rel_estm iszero). Supplementally, by the moment the node of the k-th mode becomesthe reset mode, Wk should have become zero.

Incidentally, the aforesaid finite stabilization function generator(Japanese Unexamined Patent Application Publication No. H5-324115) maybe used in the processing for gradually resetting Zk_rel_estm to zero.

Further, if it is determined to be “Others” in S6620 of FIG. 49 (e.g.,if the modes of the three child nodes are all hold mode), then the sameprocessing as that of the aforesaid S6606 (refer to FIG. 52) is carriedout in S6620.

As described above, only in a case where the modes of two or more childnodes are ready out of the modes of the three child nodes of the n-thnode, and there is no danger in that the divergence takes place in theprocessing of estimating node relative floor height errors, thesubstantial estimation processing of the node relative floor heighterrors of the two or more child nodes is carried out. And, even if themodes of two or more child nodes are all ready, if there is a danger ofthe occurrence of divergence of estimation processing, then the valuesof the estimated node relative floor height errors of the three childnodes are hold.

As described above, in S60 to S66 of FIG. 41, estimated n-th relativefloor height errors Zn_rel_estm (n=1, 2, . . . , last node number) aredetermined.

Incidentally, the relative floor height error of the root node is zero.

In the processing of the flowchart of FIG. 41, lastly, in S72, the sumof the estimated relative floor height errors of all ancestor nodes ofan n-th node (n=1, 2, . . . , last leaf node number) and an estimatedn-th node relative floor height error is determined, and the determinedsum is defined as an estimated n-th ground contact portion floor heighterror (estimated n-th floor height error) Zfn_estm.

The above is the subroutine processing of the estimation processing offloor height errors in FIG. 40.

Next, returning to the explanation of FIG. 35, the estimated n-th floorheight error Zfn_estm (n=1, 2, . . . , last leaf node number)sequentially determined by the floor configuration estimator 130 asdescribed above is added to the corrected desired ground contact portionposition/posture of each ground contact portion 10 by an adder 132 so asto determine corrected desired ground contact portion position/posturewith floor configuration error compensation. Then, the corrected desiredground contact portion position/posture with floor configuration errorcompensation are input, in place of corrected desired ground contactportion position/posture, to the corrected desired ground contactportion position/posture with deformation compensation calculator 114 h.

As the processing of the control device 50 including the floorconfiguration estimator 130 and the adder 132 explained above isrepeatedly carried out at each control cycle of the control device 50,the estimated n-th floor height error Zfn_estm converges to an actualn-th floor height error. Further, desired ground contact portionpositions/postures are corrected on the basis of the estimated n-thfloor height errors Zfn_estm corresponding to the ground contactportions so as to absorb an influence exerted by an actual n-th floorheight error on an actual floor reaction force.

If all n-th ground contact portions (n=1, 2, . . . , last leaf nodenumber) are in contact with the ground, then the point corresponding toa desired n-th ground contact portion floor reaction force central pointQn on the actual ground contact surface of an n-th ground contactportion should agree with an actual n-th floor contact point Dn_act.Therefore, the aforesaid instantaneous n-th node relative floor heighterror Zn_rel′ should not change.

In actuality, however, the instantaneous n-th node relative floor heighterror Zn_rel′ obtained by the aforesaid calculation algorithm of theaforesaid floor configuration estimator 130 significantly changes due tothe vibrations and electrical noises of a mechanical system or controldevice of the robot 1. Therefore, if the instantaneous n-th noderelative floor height error is directly used as an estimated n-th noderelative floor height error, and based on this, the estimated n-th floorheight error Zfn_estm is determined (that is, the sum of theinstantaneous relative floor height errors of all ancestor nodes of ann-th node (n=1, 2, . . . , last leaf node number) and the instantaneousn-th node relative floor height error is defined as the estimated n-thfloor height error Zfn_estm), and further, the result obtained by addingthe estimated n-th floor height error Zfn_estm to the corrected desiredground contact portion position/posture is supplied to the correcteddesired ground contact portion position/posture with deformationcompensation calculator 114 h, as shown in FIG. 35, then the groundcontact portions 10 of the robot 1 will oscillate or severely vibrate.

Meanwhile, in the intra-group complete estimation processing for a nodehaving two child nodes, the intra-group complete estimation processingfor a node having three child nodes, and the intra-group partialestimation processing for a node having three child nodes describedabove, the transfer function from the instantaneous n-th node relativefloor height error Zn_rel′ to the estimated n-th node relative floorheight error Zn_rel_estm will be a primary delay low-pass filter. Inother words, the estimated n-th node relative floor height errorZn_rel_estm will be the one obtained by passing the instantaneous n-thnode relative floor height error Zn_rel′ through a low-pass filter.Therefore, even when the estimated n-th floor height error Zfn_estmdetermined on the basis of the estimated n-th node relative floor heighterror Zn_rel_estm is added to the corrected desired ground contactportion position/posture, as shown in FIG. 35, the oscillation orvibration of the ground contact portions 10 will hardly occur. Thismakes it possible to restrain the ground contact portions 10 fromoscillating or vibrating.

Incidentally, in the present embodiment, the time constant of theaforesaid low-pass filter is Testm. Other than by the processing of thefloor configuration estimator 130 explained in the present embodiment,it is possible to accomplish a construction in which the transferfunction from the instantaneous n-th node relative floor height errorZn_rel′ to the estimated n-th node relative floor height errorZn_rel_estm will be a low-pass filter.

The characteristics of the floor configuration estimator 130 in thepresent embodiment will now be explained.

Even if each node compensating angle is changed and the correcteddesired ground contact portion position/posture with deformationcompensation, which are the final desired ground contact portionposition/posture, are changed to generate node compensating momentsrequired for the stabilization control of the posture of the robot 1,the point corresponding to the desired n-th ground contact portion floorreaction force central point Qn of the n-th ground contact portion 10 onan actual ground contact surface remains in agreement with the actualn-th floor contact point Dnact as long as the ground contact portions 10are actually in contact with the ground. Instead, the compliancemechanism 42 or the like of each leg deforms or the position/posture ofthe body 24 change. In other words, the compliance mechanism 42 or thelike deforms or the position/posture of the body 24 change in order tocancel the change of the corrected desired ground contact portionposition/posture with deformation compensation.

The floor configuration estimator 130 in the present embodiment cancelsthe influences exerted on the estimated n-th floor height error Zfn_estmby the deformation amount of the compliance mechanism 42 or the like,which is calculated by the mechanism compliance model 134 on the basisof an actual floor reaction force, and the body posture inclinationerror θberr by the time the estimated n-th floor height error Zfn_estm(n=1, 2, . . . , last leaf node number) is calculated from the correcteddesired ground contact portion position/posture with deformationcompensation. Hence, even if a body posture inclination error occurs orcorrected desired ground contact portion position/posture withdeformation compensation change, the estimated n-th floor height errorZfn_estm is not influenced thereby.

This means that the processing for estimating a floor configuration isnot subjected to interference by the compliance control or posturecontrol of the robot 1.

Therefore, even when the estimated n-th floor height error Zfn_estm isadded to the corrected desired ground contact portion position/posture,as shown in FIG. 35, the stability allowance (resistance to oscillation)of the control device of the robot 1 hardly reduces. In other words,control and processing will not interfere with each other and causeoscillation even if the compliance control, the posture control and thefloor configuration estimation processing and the operation forcorrecting a desired gait of the robot 1 by using the estimated value ofa floor configuration error are simultaneously performed. This means, inbrief, that the leg compensating operation to cancel the influenceexerted on a floor reaction force by a floor configuration error (theoperation for correcting the position/posture of a ground contactportion) can be simultaneously performed while simultaneously estimatinga plurality of parameters of the floor configuration error. Moreover,the leg compensating operation for posture control (the operation forcorrecting the position/posture of a ground contact portion) can besimultaneously performed.

Further, if the estimated n-th floor height error Zfn_estm of eachground contact portion 10 is added to the corrected desired groundcontact portion position/posture, as shown in FIG. 35, then even ifthere is actually an n-th floor height error, the influence thereof canbe offset, so that the actual total floor reaction force will steadilybe the same as that in a case where the robot 1 is traveling on asupposed floor. Furthermore, the estimation of the estimated n-th floorheight error Zfn_estm and the correction of corrected desired groundcontact portion position/posture by using the estimated n-th floorheight error Zfn_estm are sequentially carried out for each controlcycle; therefore, even if a floor configuration changes in the middle,the n-th floor height error after the change can be estimated and theinfluence exerted by the change in the floor configuration can becancelled on the basis of the estimated value. However, the floorconfiguration estimator 130 includes a low-pass filter, as mentionedabove, so that an actual total floor reaction force is transitionallysubject to the influence of a floor configuration error immediatelyafter the ground contact portions 10 come in contact with the ground orif the floor configuration changes in the middle. Thereafter, however,the influence is attenuated by the time constant Testm.

Further, there are the following characteristics.

-   1) Control is resistant to failure even if a floor configuration    error is large. This is because the hierarchical compliance control    does not carry out approximation limited to minute compensating    angles.-   2) The accuracy of estimating floor configuration errors is high.-   3) Resistance to the occurrence of oscillation attributable to the    interference with the robot posture stabilization control,    permitting a shorter time constant for estimation processing to be    achieved. This makes it possible to enhance the responsiveness of    the leg compensating operation to offset the influence exerted by a    floor configuration error on a floor reaction force, so that the    transitional influence exerted by the floor configuration error on    the floor reaction force can be quickly eliminated.

If there is no correlation in the relationship between the estimatedn-th floor height error in a landing period of one step before (a periodin which a certain ground contact portion 10 comes in contact with theground) and the estimated n-th floor height error in a landing periodfollowing the above landing period of one step before when a robot istraveling, then it is preferred to forcibly and gradually converge theestimated n-th floor height error to zero, as shown in the presentembodiment. However, if, for example, it is known that the undulation ofan actual floor surface not considered in a supposed floor is gentle,then it is considered that there is some correlation in the aforesaidrelationship. In this case, the estimated n-th floor height error in thecurrent landing period may be forcibly and gradually converged to thevalue obtained by multiplying the estimated n-th floor height error inthe landing period of one step before by a positive constant that issmaller than 1.

Further, the convergence desired value of an estimated n-th floor heighterror in the current landing period may be determined by using not onlythe estimated n-th floor height error in the landing period of one stepbefore but also the estimated n-th floor height error in the landingperiod of a plurality of steps before. Further, the convergence desiredvalue may be determined by using the estimated n-th node floor heighterror and the estimated n-th node floor inclination error of anothernode in the landing period of current step, one step before, or aplurality of steps before.

Since the present embodiment (the third embodiment) has been constructedas described above, the estimation of floor configurations,specifically, the floor height errors associated with ground contactportions, can be simultaneously, in other words, compositely, estimatedwith high accuracy even in a robot having many ground contact portions,which has been difficult to accomplish by conventional technologies.Moreover, if the ground contact portions of a robot are provided suchthat their postures can be controlled, and floor inclination errors areestimated in addition to floor height errors in the processing forestimating floor configurations, then the floor inclination errors canbe also simultaneously estimated with high accuracy.

Furthermore, by correcting a desired ground contact portionposition/posture trajectory on the basis of the result of estimating afloor configuration, even if a floor configuration is different from asupposed configuration, the influence thereof can be absorbed, making itpossible to generate a floor reaction force as desired.

In particular, the steady error of an actual floor reaction force from acontrol desired value, which has not been completely removed by thehierarchical compliance control that does not include corrections basedon the estimated values of floor configuration errors, can be broughtclose to zero as much as possible. In other words, the steady errors offloor reaction forces caused by floor configuration errors can beeliminated.

Furthermore, the arrangement has been made such that the estimation of afloor configuration is interrupted in a situation wherein the accuracyof estimating a floor configuration may deteriorate or a situationwherein the estimated value thereof may diverge. This adds to theaforesaid advantages by preventing estimated values from beinginappropriate.

Incidentally, the floor configuration estimator 130 of a robot accordingto the present embodiment is expected to estimate floor configurationsby the aforesaid techniques, but it is not essential for the floorconfiguration estimator 130 to correct motions on the basis of estimatedvalues.

Fourth Embodiment

A control device (a floor configuration estimator in particular) of amobile robot according to a fourth embodiment of the present inventionwill now be explained. In the fourth embodiment, to explain it in brief,the block diagram of the floor configuration estimator 130 shown in FIG.40 of the aforesaid third embodiment has been equivalently converted.More specifically, the floor reaction force of each node is estimated onthe basis of the estimated value of a floor configuration estimatedimmediately before (the past value of an estimated value, such as theestimated value in the last control cycle), a desired gait that has beenfinally corrected and determined (detailedly, corrected desired groundcontact portion position/posture with deformation compensation includinga compliance operation)(or an actual joint displacement), and an actualbody posture inclination error θberr. Based on the difference betweenthe estimated value and the actual floor reaction force of each node,the correction amount candidate value of the estimated value of thefloor configuration is determined so that the difference approximateszero. If it is determined that the estimation of the floor configurationwill not diverge, then the estimated value of the floor configurationthat has been estimated immediately before is corrected on the basis ofthe correction amount candidate value of the estimated value of thefloor configuration. This processing corresponds to a hierarchical floorconfiguration estimating means that is an advanced version of the meansfor estimating a floor inclination related to each foot such that thedifference between the supposed moment about the floor reaction forcecentral point of the foot of each leg of a biped mobile robot and anactual moment approximates zero and the means for estimating theinterference angle between both legs such that the difference betweenthe supposed moment about a total floor reaction force central point andan actual moment approximates zero in the Japanese Unexamined PatentApplication Publication No. H10-277969 previously proposed by thepresent applicant.

Thus, the present embodiment differs from the third embodiment only inthe processing of a floor configuration estimator 130, so that theexplanation of the processing of those other than the floorconfiguration estimator 130 will be omitted.

FIG. 57 is a block diagram showing the functional means of the floorconfiguration estimator 130 in the present embodiment. Referring to thisFIG. 57, the floor configuration estimator 130 in the present embodimentwill be explained below.

First, based on corrected desired n-th ground contact portionposition/posture with deformation compensation (n=1, 2, . . . , lastleaf node number) of each ground contact portion 10, the desired n-thground contact portion floor reaction force central point of the groundcontact portion 10 observed from the desired ground contact portionposition of each ground contact portion 10, and the aforesaid postureinclination error θberr, the corrected desired n-th ground contactportion position/posture with deformation compensation is rotationallymoved by θberr, a desired total floor reaction force central point beingthe center of the rotation, so as to determine the corrected desiredn-th ground contact portion position/posture (n=1, 2, . . . , last leafnode number) with deformation compensation after rotation. A supposedn-th floor contact position Dn (n=1, 2, . . . , last leaf node number)on a desired gait is subtracted from the determined corrected desiredn-th ground contact portion position/posture (n=1, 2, . . . , last leafnode number) with deformation compensation after rotation so as todetermine an n-th ground contact portion interference height Zn_int.This is supplied to a mechanism compliance model (inverse model)provided in a floor configuration estimator 130, thereby determining anestimated n-th ground contact portion floor reaction force, which is theestimated value of the floor reaction force of each ground contactportion 10.

Incidentally, the mechanism compliance model here is used to determinethe mutual relative relationship among the estimated ground contactportion floor reaction forces on the basis of the mutual relativerelationship among ground contact portion interference heights, and itfinds no meaning in absolute values. Further, if the detected value ofan actual floor reaction force Fn_act of an n-th ground contact portionis smaller than a certain threshold value Fn_min3, then it is assumedthat the n-th ground contact portion is not in contact with the groundalso in the aforesaid mechanism compliance model.

Further, the aforesaid estimated n-th ground contact portion floorreaction force is subtracted from the detected value of the actual floorreaction force Fn_act of an n-th ground contact portion (n=1, 2, . . . ,last leaf node number) to determine the estimated error of the n-thground contact portion floor reaction force Ffn_estm_err.

The estimated error of the n-th ground contact portion floor reactionforce Ffn_estm_err is expressed in terms of a force; therefore, it isconverted into a height error by a conversion value Cn (e.g., thereciprocal of a spring constant) and the result is adopted as an n-thground contact portion floor height error correction amount candidatevalue Zfn_inc_cand. Incidentally, the conversion value Cn is notnecessarily a diagonal matrix.

Subsequently, hierarchical relativization is performed by thehierarchical relativization processing described above on the basis ofthe determined n-th ground contact portion floor height error correctionamount candidate value Zfn_inc_cand so as to determine an n-th noderelative floor height error correction amount candidate valueZn_inc_cand (n=1, 2, . . . , last node number). Then, based on the n-thnode relative floor height error correction amount candidate valueZn_inc_cand, an estimated floor height error (estimated n-th floorheight error) Zfn_estm (n=1, 2, . . . , last leaf node number) isdetermined by the subroutine processing of the estimation processing offloor height errors. The subroutine processing of the estimationprocessing of floor height errors here is identical to the processing ofS56 to S72 shown in FIG. 41.

It is desirable to carry out the ground contact portion floorinclination error estimation processing as explained in conjunction withS70 of FIG. 41 in the case of a robot that has feet whose postures canbe controlled as ground contact portions and is capable of estimatingthe floor inclination errors in ground contact portions, as disclosed inJapanese Unexamined Patent Application Publication No. H10-277969.

Here, the third embodiment and the fourth embodiment (the presentembodiment) will be compared. The fourth embodiment is, in brief, theconverted equivalent of the block diagram of the floor configurationestimator 130 of the third embodiment. Hence, the advantages of thefourth embodiment are the same as the advantages of the thirdembodiment. Further, as with the third embodiment, it is not essentialto correct motions on the basis of the estimated values of floorconfigurations.

In the third and the fourth embodiments, distributed pressure sensorsmay be used as floor reaction force sensors. Distributed pressuresensors make it possible to estimate more finely the deformations ofground contact portions due to actual floor reaction forces than forcesensors, such as the six-axis force sensor 34, so that the accuracy inestimating floor configurations can be improved.

Furthermore, as shown in FIG. 1 of Japanese Patent No. 3035051 by thepresent applicant, a plurality of contact sensors may be disposed on aground contact portion (e.g., on the four corners of the ground contactportion), and it may be determined in which direction the ground contactportion is likely to float on the basis of the output signals thereof.And, the estimation of a floor configuration in the direction in which acontact sensor that is not in contact with the ground is further likelyto float may be interrupted, or the estimation of a floor configurationmay be also interrupted when all contact sensors are floating.

Fifth Embodiment

In the first to the fourth embodiments described above, the explanationshave been given to the cases where the total number of child nodes of anode having child nodes is three or less have been taken as examples;however, also in a case where the number of child nodes is four or more,the concepts of the moments and inclination angles can be expanded asshown below by introducing the concept of a weighted average.

The concept can be expanded for any of desired values, actual values,and errors. Here, therefore, the desired values, the actual values, anderrors will not be discriminated; instead, the moment for a general setof ground contact portion floor reaction forces Fn (n=1, 2, . . . ) willbe expanded and defined as shown below.

A set of the floor reaction forces of an n-th ground contact portion Fn(n=1, 2, . . . , last leaf node number) is hierarchically relativized todetermine an n-th node relative floor reaction force Fn_rel (n=1, 2, . .. , last node number).

The number of the child nodes of an n-th node is denoted by r.

The identification number of an j-th child node of the n-th node isdenoted by aj (j=1, 2, . . . , r).

The weight of the j-th child node of the n-th node is denoted by Wj(j=1, 2, . . . , r).

The column vector having, as a j-th element, the weight of the j-thchild node of the n-th node is denoted by UWn.

Thus, UWn is defined by the following expression 38, where “T” meanstransposition, and a column vector can be expressed by transpose a rowvector.UWn=(Wa1, Wa2, . . . , War)^(T)  Expression 38

r−1 mutually independent vectors orthogonal to the vector UWn (that is,the scalar product with the vector UWn is zero) are denoted by R(1),R(2), . . . , R(r−1). R(j) (j=1, 2, . . . , r−1) denotes the columnvector of r row, 1 column. Incidentally, R(j) (j=1, 2, . . . , r−1) aredesirably orthogonal to each other, considering the ease of computationand control accuracy. A matrix having R(j) set to a j-th column (j=1, 2,. . . , r−1) is denoted by Hn. Hn is the matrix of r row, r−1 column.

A column vector whose j-th element (j=1, 2, . . . , r) is a relativefloor reaction force Faj_rel of a j-th child node of the n-th node isdefined as an n-th group relative floor reaction force vector Fn_rel_c.

In other words, Fn_rel_c is defined by the following expression 39.Fn_rel_c=(Fa1_rel, Fa2_rel, . . . , Far_rel)^(T)  Expression 39Fn_rel_c denotes a column vector of r row, 1 column.

A column vector Mn_exp satisfying the following expression 40 isreferred to as an n-th node expansion floor reaction force moment.Fn _(—) rel _(—) c=Hn*Mn_exp  Expression 40Mn_exp denotes a column vector of r−1 row, 1 column.

For the set of ground contact portion floor reaction forces Fn (n=1, 2,. . . ), the n-th node expansion floor reaction force moment Mn_exp isdefined as described above.

More specifically, based on the ground contact portion floor reactionforce Fn (n=1, 2, . . . ), a hierarchized node relative floor reactionforce Fn_rel (n=1, 2, . . . ) is determined, and further, a vectorhaving, as its element, the coefficient of the linear combination when avector (Fa1_rel, Fa2_rel, . . . , Far_rel)^(T) having, as its elements,the relative floor reaction forces of all child nodes aj (j=a1, a2, . .. , ar) of an n-th node is expressed by the liner combination of certainpredetermined mutually independent vectors R(j) (j=1, 2, . . . , r−1)that are orthogonal to a vector (Wa1, Wa2, . . . , War)^(T) having theweights of all child nodes of the n-th node as its elements, is referredto as the n-th node expansion floor reaction force moment Mn_exp.

Incidentally, a j-th element (the element of a j-th row) of the n-thnode expansion floor reaction force moment Mn_exp is referred to as thej-th component of the n-th node expansion floor reaction force moment.

Further, the n-th node expansion floor reaction force moment relative tothe set of actual ground contact portion floor reaction forces Fn (n=1,2, . . . ) is referred to as the actual n-th node expansion floorreaction force moment Mn_exp_act.

Further, the n-th node expansion floor reaction force moment relative tothe set of desired ground contact portion floor reaction forces Fn (n=1,2, . . . ) is referred to as the desired n-th node expansion floorreaction force moment Mn_exp_ref. The desired n-th node expansion floorreaction force moment is usually set to a zero vector.

Further, the n-th node expansion floor reaction force moment relative tothe set of ground contact portion floor reaction force errors Fn (n=1,2, . . . ) is referred to as the n-th node expansion floor reactionforce moment error Mn_exp_err.

Similarly, the inclination angle relative to a set of general groundcontact portion heights Zn (n=1, 2, . . . ) is expanded and defined asfollows.

A set of ground contact portion heights Zn (n=1, 2, . . . , last leafnode number) is hierarchically relativized to determine an n-th noderelative height Zn_rel (n=1, 2, . . . , last node number).

A column vector whose j-th element (j=1, 2, . . . , r) is a relativeheight Zaj_rel of a j-th child node of the n-th node is defined as ann-th node relative height vector Zn_rel_c.

In other words, Zn_rel_c is defined by the following expression 41.Zn_rel_c=(Za1_rel, Za2_rel, . . . , Zar_rel)^(T)  Expression 41

A column vector θn_exp satisfying the following expression 42 isreferred to as an n-th node expansion floor reaction force inclinationangle.Zn _(—) rel _(—) c=Hn*θn _(—exp)  Expression 42

θn_exp denotes a column vector of r−1 row, 1 column.

More specifically, based on the ground contact portion height Zn (n=1,2, . . . ), a hierarchized node relative height Zn_rel (n=1, 2, . . . )is determined, and further, a vector having, as its element, thecoefficient of the linear combination when a vector (Za1_rel, Za2_rel, .. . , Zar_rel)^(T) having, as its elements, the relative heights of allchild nodes aj (j=1, 2, . . . , r) of an n-th node is expressed by theliner combination of certain predetermined mutually independent vectorsR(j) (j=1, 2, . . . , r−1) that are orthogonal to a vector (Wa1, Wa2, .. . , War)^(T) having the weights of all child nodes of the n-th node asits elements, is referred to as the n-th node expansion inclinationangle θn.

By using the expansion floor reaction force moment and the expansioninclination angle defined as described above, the compliance control andthe processing for estimating floor configurations are expanded almostas they are for a case where the number of the child nodes of an a-thnode is four or more. The following will explain an example thereof as afifth embodiment. In the fifth embodiment, the functional constructionof a control device of a robot is the same as that shown in theaforesaid FIG. 2. In this case, the functional construction of ahierarchical compliance operation determiner may be the same as thatexplained with reference to the aforesaid FIG. 35 except for acompensating total floor reaction force moment distributor, acompensating angle determiner, and a floor configuration estimator.

[Expanding the Processing of the Compensating Total Floor Reaction ForceMoment Distributor]

For example, the processing of the compensating total floor reactionforce moment distributor in the hierarchical compliance operationdeterminer is expanded as shown below.

The expansion floor reaction force moment to be generated in addition toa desired n-th node expansion floor reaction force moment Mn_exp_rel(normally zero) to restore the posture of a robot is referred to as ann-th node compensating expansion floor reaction force moment Mn_exp_dmd.

Expression 43 given below holds between the n-th node compensatingexpansion floor reaction force moment Mn_exp_dmd and the n-th nodecompensating floor reaction force moment Mn_dmd. However, Cn_mechdenotes a predetermined coefficient matrix (the matrix of 2 rows, r−1column) determined by a desired node floor reaction force central pointand the compliance characteristic of the mechanism of the robot.Mn_dmd=Cn—mech*Mn_exp_dmd  Expression 43

Meanwhile, generalizing the aforesaid expression 10 provides thefollowing expression 44.Mdmd=ΣMn_dmd  Expression 44

where Σ means the total sum on all n-th nodes.

Hence, the compensating total floor reaction force moment distributormay determine the n-th node compensating expansion floor reaction forcemoment Mn_exp_dmd (n=1, 2, . . . ) and the n-th node compensating floorreaction force moment Mn_dmd such that expression 43 and expression 44are satisfied.

[Expanding the Processing of the Compensating Angle Determiner (θnDeterminer)]

The processing of the n-th node compensating angle determiner in thefifth embodiment is carried out as follows. For example, in theprocessing of the θ145 determiner in the third embodiment (refer to FIG.25 and FIG. 33), the processing until M145 act is determined is replacedby the processing for determining the aforesaid actual n-th nodeexpansion floor reaction force moment Mn_exp_act on the basis of theactual floor reaction force of each ground contact portion, 145 isreplaced by n, and the dimension of the compensation filter, thelow-pass filter, and gain Kn in FIG. 33 may be expanded to the numberobtained by subtracting 1 from the number of child nodes of an n-thnode. The compensating angle θn of the n-th node can be determined bythe processing that has been replaced as described above.

[Estimating a Floor Configuration by Using an Expansion Floor ReactionForce Moment]

As described below, the processing of the floor configuration estimatormay partly incorporate a technique using the concept of the expansionfloor reaction force moment.

In place of the processing shown in FIG. 56 of the floor configurationestimator explained in the aforesaid fourth embodiment, the processingshown in the block diagram of FIG. 57 is used to estimate floorconfiguration errors.

Referring to FIG. 57, this processing will be explained. First, an n-thnode expansion floor reaction force moment estimation errorMn_exp_estm_err corresponding to an n-th ground contact portion floorreaction force estimation error Ffn_estm_err (n=1, 2, . . . ) isdetermined according to the definition of the expansion moment from thedifference between the estimated n-th ground contact portion floorreaction force calculated as explained in conjunction with the aforesaidFIG. 56 and the actual floor reaction force of the n-th ground contactportion (the set of differences for each node). Incidentally, theaforesaid vector R(j) required to determine the n-th node expansionfloor reaction force moment estimation error is determined on the basisof the weight of each node.

Subsequently, as shown in expression 45 given below, the n-th nodeexpansion floor reaction force moment estimation error Mn_exp_estm_erris multiplied by a predetermined coefficient matrix Kn_cmpl thereby todetermine an n-th node expansion inclination angle correction amountcandidate value θn_inc_cand, which is the candidate value of thecorrection amount of the n-th node expansion inclination angle.θn _(—inc)_cand=Kn_cmpl*Mn_exp_estm_err  Expression 45

Incidentally, Kn_cmpl is not necessarily a diagonal matrix.

Subsequently, a coefficient for a vector R(j) (j=1, 2, . . . r−1) isdefined as the j-th element of θn_inc_cand to determine a vectorobtained by linearly combining the vectors R(j). The determined vectoris defined as the n-th node relative floor height error correctionamount candidate value Zn_inc_cand, which is the candidate value of thecorrection amount of the n-th node relative floor height error.

In other words, Zn_inc_cand is determined according to the followingexpression 46.Zn_inc_cand=Hn*θn_inc_cand  Expression 46

As described above, the processing for determining the n-th node floorreaction force estimation error Fn_estm_err to the n-th node relativefloor height error correction amount candidate value Zn_inc_cand may bereplaced from the one shown in FIG. 56 to the one shown in FIG. 57.

The arithmetic processing of FIG. 57 other than the above is the same asthe arithmetic processing of FIG. 56.

The subroutine processing of the floor height error estimationprocessing in the fifth embodiment requires the processing for a casewhere the number of child nodes of an n-th node is four or more. Thisprocessing expands the aforesaid floor configuration estimationprocessing for three child nodes (refer to FIG. 41), and if the groundcontact portion corresponding to a child node j is likely to float anddiverge, then the estimated j-th node relative floor height errorZj_rel_estm is maintained at the last value Zj_rel_estm_p (the j-th noderelative floor height error correction amount candidate valueZj_inc_cand is set to zero), as with the floor configuration estimationprocessing for three-children nodes. Further, the estimated noderelative floor height errors Zk_rel_estm of the remaining child nodesare determined such that they approximate the value obtained by adding acertain predetermined bias value c to the sum of the last estimated k-thnode relative floor height error Zk_rel_estm_p and Zn_inc_cand. However,the bias value c is determined such that the weighted average of theestimated node relative floor height errors of all child nodes of then-th node is zero.

As described above, the floor configuration estimation processing can beexpanded also when there is a node having four or more child nodes.However, the arithmetic processing will be easier if a hierarchicalstructure is determined not to include four or more child nodes ratherthan carrying out such processing.

Sixth Embodiment

Referring now to FIG. 58 to FIG. 65, a control device for a mobile robotaccording to a sixth embodiment of the present invention will beexplained. FIG. 58 and FIG. 59 are side views showing a legged mobilerobot 51 according to the sixth embodiment (a bipedal mobile robot inthe present embodiment), the portion of a knee joint 56, which will bediscussed later, being in contact with a floor (kneeling). Incidentally,FIG. 58 schematically shows the robot 51 as a linear link assembly.

An object of the invention related to the present embodiment is tostably control the posture of a robot by manipulating a reaction force(external force) received from a floor, including an object fixed on thefloor, in a state wherein a portion, such as a knee, an elbow, or atorso other than the distal portions of the legs or arms of the robot isin contact with the floor or the object fixed (set) on the floor (anobject regarded as a material extended from the floor).

More specifically, the object is to permit posture control in a statewherein the robot is kneeling or in a state wherein it is sitting on achair.

A conventional publicly known human-like robot is not provided withsensors for detecting floor reaction forces on, for example, its knees,so that it has been impossible to control the floor reaction forcesacting on the knees. For instance, in the conventional human-like robot,when the robot is kneeling as shown in FIG. 58 and FIG. 59, the floorreaction forces acting on the right and left knees are dependent on(governed by) the concavities and convexities of a floor, and the jointangles of legs have not been adjusted on the basis of the concavitiesand convexities of the floor. Hence, there have been some cases wherethe portions of the robot in contact with the floor shake due toconcavities and convexities of the floor, leading to unstable groundcontact states of the ground contact portions. And, in such a state, anattempt to make the robot move or work sometimes has caused instabilityof the posture of the robot.

Further, when, for example, the robot stands up from the state whereinit is kneeling, there has been a danger of the robot falling down if anactual floor inclination is different from the inclination supposed in adesired gait, because the position of the center-of-gravity of the robotis not properly controlled and the robot tries to stand up with its bodyposture inclined.

Further, in a case where the robot sitting on a chair operates or workswhile maintaining its posture stable, it is preferred that not only thefloor reaction forces acting on the feet of the legs of the robot butalso the reaction forces acting from the chair onto the buttocks of therobot can be properly controlled so as to restore the posture of therobot to a proper posture when it stands up from the chair or sits ontothe chair.

Conceptually, if an object, such as a chair, which is fixedly set on afloor is considered as a part of a floor, then the aforesaid reactionforces may be said to be floor reaction forces in a broad sense.

With the view of the problems with the conventional system as describedabove, the sixth embodiment solves the aforesaid problems, and in astate wherein portions of a mobile robot, such as the knees, elbows, thetorso, and buttocks, other than the distal portions of legs or arms arein contact with a floor or an object considered as a material extendedfrom the floor and subjected to reaction forces, it properly controlsnot only the reaction forces acting on the distal portions of the legsor arms of a robot but also the reaction forces acting on the portionsother than the distal portions of the legs or arms by using the controlof the hierarchical compliance described above, thereby making itpossible to keep the posture of the mobile robot stable.

The following will explain the sixth embodiment in more detail by takingthe robot (bipedal mobile robot) 51 shown in FIG. 58 and FIG. 59 as anexample.

First, the mechanical construction of the robot 51 will be explained. Inthe robot 51, two legs (link mechanisms) 52 (one leg being not shown)are extended from the bottom end of a body (base) 53. Further, two arms(link mechanisms) 54 (one arm being not shown) are extended from upperpart of the body 53. Each leg 52 is provided with a hip joint 55, a kneejoint 56, and an ankle joint 57 at its proximal portion adjacent to thebody 53 (the portion connected to the body 53), its middle portion, andits distal portion, respectively, which are operated by actuators, suchas electric motors, and a foot 58 is connected to the distal portion ofeach leg 52 through the ankle joint 57. Similarly, each arm 54 isprovided with a shoulder joint 59, an elbow joint 60, and a wrist joint61 at its proximal portion adjacent to the body 53 (the portionconnected to the body 53), its middle portion, and its distal portion,respectively, which are operated by actuators, such as electric motors,and a hand 62 is connected to the distal portion of each arm 54 throughthe wrist joint 61. In this example, the joints 55, 56, and 57 of eachleg 2 are joints having, for example, 3 degrees of freedom, 1 degree offreedom, and 2 degrees of freedom, respectively, and the foot 58 has 6degrees of freedom relative to the body 53. Further, the shoulder joint59, the elbow joint 60, and the wrist joint 61 of each arm 54 areconstructed such that their degrees of freedom allow the hand 62 to havea degree of freedom of 6 degrees or more with respect to the body 53.

Although not shown, the body 53 of the robot 51 is provided with acontrol device 50 similar to that of the aforesaid first embodiment.Further, a head 63 is provided at the upper end of the body 53.

In this robot 51, in the state wherein the robot 51 is on its knees(particular posture state), as shown in FIG. 58 and FIG. 59, theportions of the foot 58 of each leg 52 and each knee joint 56 (morespecifically, the surface portion of a link (shank link) connecting theknee joint 56 and the ankle joint 57 at near the knee joint 56;hereinafter it will be referred to simply as the knee) and the hand 62of each arm 54 are ground contact portions. And, in the presentembodiment, as shown in FIG. 59, the knee, which is a ground contactportion, is provided with a floor reaction force sensor 90 (loadsensor). The floor reaction force sensor 90 is constructed of a mainbody (sensor part) 92 and a soft member (elastic member) 94, such as asponge. The main body 92 is fixed to the knee (leg link), and theoutside of the main body 92 is covered with a soft member (elasticmember) 94. To enhance the accuracy of the compliance control in theknee, it is desirable to shape the surface (ground contact surface) ofthe soft member 94 into a round convex surface in addition to coveringthe knee with the soft member 94. This arrangement reduces thenonlinearity of the relationship between a corrective operation of adesired motion of the robot 51 and a floor reaction force, resulting inbetter control performance of the compliance control. Incidentally,although not shown, the foot 58 and the ankle joint 57 are connectedthrough a floor reaction force sensor, such as a six-axis force sensor,and the compliance mechanism. Similarly, the hand 62 and the wrist joint61 are connected through a floor reaction force sensor, such as asix-axis force sensor, and the compliance mechanism, which are notshown. The connecting constructions may be ones that are publicly known.

The floor reaction force sensor 90 of the ground contact portion of theknee joint may be any one of the following.

-   1) Sensor that detects only the translational force in the direction    perpendicular to the surface of a ground contact portion that comes    in contact with a floor (ground contact surface).-   2) Sensor that detects not only the translational force in the    direction perpendicular to the surface of a ground contact portion    that comes in contact with a floor (ground contact surface) but also    the translational force in a direction other than the direction    perpendicular to the contact surface.-   3) Distributed pressure sensor-   4) Sensor that detects moments in addition to translational forces    (e.g., six-axis force sensor)

Alternatively, a displacement sensor for detecting the deformation(distortion) of the aforesaid soft member 94 may be used in place of onethat directly detects a load.

In the present embodiment, as the floor reaction force sensor 90, thesensor of the above 2) will be used.

To further enhance the accuracy of the compliance control, the sensor ofthe above 3) or 4) may be used as the floor reaction force sensor 90.Incidentally, as the structure of the knee, a floor reaction forcesensor may be added to a protective pad provided with a spring (a padfor protecting the knee), as disclosed in Japanese Unexamined PatentApplication Publication No. 2000-62760 previously proposed by thepresent applicant.

In the present embodiment, the hierarchical structure as shown in FIG.61 is set for the robot 51 on its knees as described above.Specifically, the right foot 58, the left foot 58, the right knee, theleft knee, the right hand 62, and the left hand 62 as the ground contactportions are associated with a first node, a second node, a third node,a fourth node, a fifth node, and a sixth node, respectively, which areleaf nodes. Further, a 12th node having the first node and the secondnode as child nodes, a 34th node having the third node and the fourthnode as child nodes, a 56th node having the fifth node and the sixthnode as child nodes, and a 1234th node having the 12th node and the 34thnode as child nodes have been set as intermediate nodes. Further, a123456th node having the 1234th node and the 56th node as child nodeshas been set as the root node. Further, according to the policy similarto that in the aforesaid first embodiment and the like, a desired floorreaction force central point Qn (n=1, 2, 3, 4, 5, 6, 12, 34, 56, 1234,123456) is associated with each node, as shown in the figure.

Incidentally, the desired floor reaction force central point Q123456 ofthe 123456th node, which is the root node, is identical to a desiredtotal floor reaction force central point P.

In the present embodiment, the control device 50 is supposed to have afunctional construction similar to that shown in the aforesaid FIG. 2.

In this case, the desired ground contact portion trajectory of a desiredmotion in a desired gait output by a gait generating device 100 in thepresent embodiment is constructed of the desired position/posturetrajectory of each hand 62, the desired position/posture trajectory ofeach foot 58, and the desired position trajectory of each knee. In thiscase, the gait generating device 100 preferentially determines desiredfoot positions/postures (desired first and second ground contact portionpositions/postures), desired hand positions/postures (desired fifth andsixth ground contact portion positions/postures), and desired kneepositions (desired third and fourth ground contact portion positions) sothat the feet 58, the hands 62, and the knees come in contact with theground on a supposed floor surface as required for the gait, thendetermines a desired ZMP (desired total floor reaction force centralpoint) in a supporting polygon, which is a minimum convex polygon thatincludes the desired ground contact point (or a desired ground contactline or a desired ground contact surface) of each ground contactportion, and then further determines desired body position/posture byusing a dynamic model of the robot 51 such that the desired footpositions/postures, the desired hand positions/postures, the desiredknee positions, and the desired ZMP are satisfied.

If the head 63 can be moved with respect to the body 53, then thedesired position/posture trajectory of the head 63 is included in adesired motion.

In the present embodiment, the arithmetic processing of the robotgeometric model (inverse kinematics calculator) 110 shown in FIG. 2differs from that of the first to the third embodiments, as will bediscussed later.

Further, in the present embodiment, a part of the processing techniqueof a hierarchical compliance operation determiner 114 differs from theone explained in the first to the third embodiments described above.FIG. 62 shows the functional construction of the hierarchical complianceoperation determiner 114 of the present embodiment. In the processing ofthis functional construction, major aspects that are different from thefirst to the third embodiments will be explained. A compensating totalfloor reaction force moment distributor 114 a determines and outputs notonly the node compensating floor reaction force moments of theintermediate nodes and the root node, but also a node compensating floorreaction force moment Mn_dmd of an n-th node (n=1, 2, 5, 6) of the leafnodes, that is, the required floor reaction force moment (the requiredmoment for restoring the posture) about a desired n-th ground contactportion floor reaction force central point (n=1, 2, 5, 6). This isbecause, in the robot 51, moments can be generated about the desiredfloor reaction force central points of the feet 58 and the hands 62 asthe ground contact portions. The technique for determining the nodecompensating floor reaction force moment Mn_dmd, including the requiredmoments, may be the same technique explained in the first and the secondembodiments described above. Further, in the present embodiment, thecompensating total floor reaction force moment distributor 114 adetermines and outputs a corrected desired n-th ground contact portionfloor reaction force moment Mn_refmdfd (n=1, 2, 5, 6) about a desiredn-th ground contact portion floor reaction force central point (n=1, 2,5, 6), which is corrected by generating a node compensating floorreaction force moment about the desired floor reaction force centralpoint of each corresponding node, together with a translational forcecomponent Fn_refmdfd (n=1, 2, 3, 4, 5, 6) of a corrected desired groundcontact portion floor reaction force. These are used by a deformationcompensation amount calculator 114 n to calculate a deformationcompensation amount.

A compensating angle determiner (the part enclosed by the virtual linein FIG. 62) in the present embodiment determines and outputs not onlythe compensating angles of the intermediate nodes and the root node butalso the node compensating angle θn of an n-th node (n=1, 2, 5, 6) amongthe leaf nodes. A On determiner that determines these n-th nodecompensating angles θn (n=1, 2, 5, 6) determines an n-th nodecompensating angle θn (n=1, 2, 5, 6) by the same processing as that ofthe foot compensating angle determiner disclosed in Japanese UnexaminedPatent Application Publication No. H10-277969. Incidentally, thetechnique for determining the compensating angles θn (n=12, 34, 56,1234, 123456) of the intermediate nodes and the root node other than theabove nodes may be the same as the technique explained in the first andthe second embodiments described above.

Further, a corrected desired ground contact portion position/posturecalculator 114 g in the present embodiment combines node rotationalmanipulations on the basis of each n-th node compensating angle θn(n=12, 34, 56, 1234, 123456) by the technique explained in the first andthe second embodiments described above so as to correct a desired groundcontact portion position, and it further corrects the desired posture ofan n-th ground contact portion (n=1, 2, 5, 6), thereby determining thecorrected desired ground contact portion position/posture of each groundcontact portion.

Further, according to the present embodiment, a floor configurationestimator 130 determines an estimated n-th floor height error Zfn_estm(n=1, 2, 3, 4, 5, 6) by the same technique as the technique explained inthe aforesaid third embodiment and also estimates a ground contactportion floor inclination error of an n-th ground contact portion θfn(n=1, 2, 5, 6). The estimation of the ground contact portion floorinclination error θfn is performed by the same processing as theprocessing for estimating foot floor inclinations by the floorconfiguration estimator disclosed in Japanese Unexamined PatentApplication Publication No. H10-277969.

The processing techniques of the functional construction of thehierarchical compliance operation determiner 114 of the presentembodiment other than the ones described above are the same as that inthe third embodiment.

Except for the different aspects explained above, the arithmeticprocessing of the control device 50 in the present embodiment is thesame as that in the first to the third embodiments.

Next, the processing of the aforesaid robot geometric model 110 in thepresent embodiment will be explained in detail.

In the robot 51 of the present embodiment, the number of the degrees offreedom of the joints from the body 53 to the knees is limited to three(only 3 degrees of freedom in the hip joints 55); therefore, it isimpossible to change only the difference in height between the right andleft knees while strictly maintaining body position/posture, footposition/posture, the horizontal positions of the right and left knees,and the sum of the heights of the right and left knees at their desiredvalues. In other words, if an attempt is made to forcibly change onlythe height difference between the right and left knees, then a twist orslippage would inconveniently take place between ground contact portions(namely, the knees and the feet) and a floor.

Similarly, it is impossible to change only the difference in heightbetween the right and left feet while strictly maintaining the bodyposition/posture, the positions of both knees, the horizontal positionsof both feet, and the sum of the heights of the right and left feet attheir desired values. In other words, if an attempt is made to forciblychange only the height difference between the right foot and left foot,then a twist or slippage would inconveniently take place between aground contact portion (namely, a knee and a foot) and a floor.

Similarly, it is impossible to change only the sum of the heights of theright and left knees while strictly maintaining the bodyposition/posture, foot positions/postures, the horizontal positions ofthe right and left knees, and the height difference between the rightand left knees at their desired values. In other words, if an attempt ismade to forcibly change only the sum of the heights of the right andleft knees, then the body position would be dislocated from a desiredposition even though a twist or slippage may not take place betweenground contact portions (namely, the knees and the feet) and a floor. Asa result, the position of the total center-of-gravity of the robot 51and an inertial force will deviate from desired values, leading todeteriorated stability of the robot 51.

This means that, in any case, the corrected desired ground contactportion position/posture with deformation compensation and desired bodyposition/posture that have been determined by the hierarchicalcompliance operation determiner 114 cannot be strictly satisfied at thesame time, so that a twist or slippage takes place between the feet 58or the knees of the robot 51 and the floor or the totalcenter-of-gravity of the robot 51 and an inertial force deviate fromtheir desired values, possibly resulting in deterioration of thestability of the robot 51 when the robot 51 is in the posture stateshown in FIG. 58 and FIG. 59. Incidentally, the hands 62 among theground contact portions have a degree of freedom of 6 degrees or more offreedom with respect to the body 53; therefore, no twist or slippagewill occur between the hands 62 and the floor unless the robot 51 ismade to take a posture in which coming into contact with the floorthrough portions other than the hands 62 of the arms 54.

Hence, in the present embodiment, the posture or the position/posture ofthe body 53 are corrected on the basis mainly of a change in the heightdifference between the right and left knees while restraining, as muchas possible, the occurrence of a twist or slippage between the knees andthe feet 58 among the ground contact portions of the robot 51 and thefloor and also restraining, as much as possible, a change in theposition of the center-of-gravity (especially the horizontal position)of the body 53.

FIGS. 63( a) and (b) show examples of the operations for correcting theposture of the body 53. FIG. 63( c) will be discussed later. These FIGS.63( a), (b), and (c) omit showing the arms 54 and the head 63.

One of the operations for correcting the posture of the body 53 is theoperation in which, from the state wherein the robot 51 is kneeling asshown in FIG. 63( a), the body 53 is rotated (rotated as indicated by anarrow y1) by using, as the axis of rotation, the trunk axis passingapproximately the center-of-gravity G of the body 53, as shown in FIG.63( b), on the basis of a change in the height difference between theright and left knees caused by compliance control.

Further, in the posture in which the robot 51 is kneeling, as shown inFIG. 63, a change in the height difference between the right and leftfeet 58 and 58 causes the height difference between the right and leftknees to change by about half the change in the height differencebetween the right and left feet 58 and 58. Hence, the same operation asthat of correcting the posture of the body 53 on the basis of a changein the height difference between the right and left knees is performedby only a half amount, as compared with the case of a change in theheight difference between the right and left feet, on the basis of achange in the height difference between the right and left feet 58 and58 caused by the compliance control.

Further, when correcting the posture of the body 53 as described above,the position and the posture of the body 53 are corrected on the basisof a change in the sum of the heights of the right and left knees causedby the compliance control while restraining the occurrence of a twist orslippage between ground contact portions (the knees and the feet 58) andthe floor as much as possible, and also restraining, as much aspossible, a change in the position (especially the horizontal position)of the center-of-gravity G of the body 53 or in the inclination of asegment connecting the center-of-gravity G and a total floor reactionforce central point.

FIG. 64 visually shows an operation for correcting the position and theposture of the body 53 on the basis of changes in the sum of the heightsof the right and left knees. Specifically, from the posture of the robot51 indicated by the dashed lines, as both knees are operated to movedown by the compliance control to the posture of the robot 51 indicatedby the solid lines, the bottom end portion (or the waist) of the body 53is shifted forward, as indicated by an arrow y3, and the inclination ofthe body 53 is shifted backward (in the direction in which the body 53rises), as indicated by an arrow y2. In other words, the body 53 istilted backward while maintaining the position of the center-of-gravityG of the body 53 (or the position of a predetermined representativepoint of the body 53), especially the horizontal position thereof.Alternatively, the body 53 is tilted backward while maintaining theinclination of the segment connecting the center-of-gravity G and thedesired total floor reaction force central point P. Further, as anoperation for raising both knees is performed by the compliance control,the bottom end portion (or the waist) of the body 53 is shiftedbackward, inversely from the above, to shift the inclination of the bodytoward the front. In other words, the body 53 is tilted forward whilemaintaining the position of the center-of-gravity G of the body (or theposition of the predetermined representative point of the body),especially its horizontal position. Alternatively, the body 53 is tiltedforward while maintaining the inclination of the segment connecting thecenter-of-gravity G and the desired total floor reaction force centralpoint P. Incidentally, Q1″ and Q3″ in FIG. 64 denote the desired floorreaction force central point of the foot 58 and the desired floorreaction force central point of the knee, respectively, after theposition/posture of the body 53 have been corrected as described above.In the example, Q1″ is identical to a desired floor reaction forcecentral point Q1 of the foot 58 before the correction.

The above is the overview of the operation for correcting the postureand the position of the body 53.

The processing function of the robot geometric model (inverse kinematicscalculator) 110 in the present embodiment is shown by the block diagramof FIG. 65.

The inverse kinematics calculator 110 first determines the correctionamount of the height difference between the right and left kneesZkneediffmdfd and the correction amount of the sum of the heights of theright and left knees Zkneesummdfd for the desired positions of the rightand left knees determined by the gait generating device 100, and thecorrection amount of the height difference between the right and leftfeet 58 and 58 Zfootdiffmdfd for the desired positions of the right andleft feet determined by the gait generating device 100 on the basis ofthe corrected desired n-th ground contact portion position/posture withdeformation compensation (n=1, 2, . . . , 6) and the desired groundcontact portion positions/postures of the feet 58 and the kneesdetermined by the gait generating device 100.

Specifically, the correction amount of the height difference between theright and left knees Zkneediffmdfd, the correction amount of the sum ofthe heights of the right and left knees Zkneesummdfd, and the correctionamount of the height difference between the right and left feetZfootdiffmdfd are determined according to the following expressions 47,48, and 49.Zkneediffmdfd=Corrected desired fourth ground contact portion positionwith deformation compensation−Desired fourth ground contact portionposition−Corrected desired third ground contact portion position withdeformation compensation+Desired third ground contact portionposition  Expression 47Zkneesummdfd=Corrected desired fourth ground contact portion positionwith deformation compensation−Desired fourth ground contact portionposition+Corrected desired third ground contact portion position withdeformation compensation−Desired third ground contact portionposition  Expression 48Zfootdiffmdfd=Corrected desired second ground contact portion positionwith deformation compensation−Desired second ground contact portionposition−Corrected desired first ground contact portion position withdeformation compensation+Desired first ground contact portionposition  Expression 49

More precisely, “portion position” in expressions 47 to 49 is the heightcomponent (the component in the vertical direction) of the portionposition.

Subsequently, the body position/posture correction amount is determinedon the basis of the correction amount of the height difference betweenthe right and left knees Zkneediffmdfd, the correction amount of the sumof the heights of the right and left knees Zkneesummdfd, and thecorrection amount of the height difference between the right and leftfeet Zfootdiffmdfd.

The body position/posture correction amount is composed of a bodyposition correction amount for knee height difference Xbkneediffmdfd, abody posture correction amount for knee height differenceθbkneediffmdfd, a body position correction amount for knee height sumXbkneesummdfd, a body posture correction amount for knee height sumθbkneesummdfd, a body position correction amount for foot heightdifference Xbfootdiffmdfd, and a body posture correction amount for footheight difference θbfootdiffmdfd. Specifically, these values aredetermined as follows.

First, based on the correction amount of the height difference betweenthe right and left knees Zkneediffmdfd, the body position correctionamount for knee height difference Xbkneediffmdfd and the body posturecorrection amount for knee height difference θbkneediffmdfd forpreventing the correction amount of the height difference between theright and left knees Zkneediffmdfd from causing a twist or slippagebetween ground contact portions (the knees and the feet 58) and a floorare determined by geometric calculation on the basis of the correctionamount of the height difference between the right and left kneesZkneediffmdfd and the desired posture (desired motion) of the robot 51at that instant (current time).

To be more specific, for example, Xbkneediffmdfd and θbkneediffmdfd aredetermined according to the following expression 50, where Kxkneediffand Kthkneediff denote proportional coefficients based on desiredpostures of the robot 51.Xbkneediffmdfd=Kxkneediff*Zkneediffmdfdθbkneediffmdfd=Kthkneediff*Zkneediffmdfd  Expression 50

As an alternative, the relationship between the correction amount of theheight difference between the right and left knees (or a 34th nodecompensating angle θ34) and the correction amounts of the bodyposition/posture may be determined beforehand for some representativedesired postures of the robot 51, and it may be stored as a map orfunction so as to determine Xbkneediffmdfd and θbkneediffmdfd on thebasis of the map or function and the correction amount of the heightdifference between the right and left knees Zkneediffmdfd. Incidentally,in the present embodiment, the body posture is corrected by rotating thebody 53 about its trunk axis on the basis of the correction amount ofthe height difference between the right and left knees, so that the bodyposition correction amount for knee height difference Xbkneediffmdfd maybe zero.

Next, in the same manner as described above, based on the correctionamount of the height difference between the right and left feet 58 and58 Zfootdiffmdfd, the body position correction amount for foot heightdifference Xbfootdiffmdfd and the body posture correction amount forfoot height difference θbfootdiffmdfd for preventing the correctionamount of the height difference between the right and left feet 58 and58 Zfootdiffmdfd from causing a twist or slippage between ground contactportions (the knees and the feet 58) and a floor are determined bygeometric calculation on the basis of the correction amount of theheight difference between the right and left feet Zfootdiffmdfd and thedesired posture (desired motion) of the robot 51 at that instant(current time).

To be more specific, for example, Xbfootdiffmdfd and θbfootdiffmdfd aredetermined according to the following expression 51, where Kxfootdiffand Kthfootdiff denote proportional coefficients based on desiredpostures of the robot 51. As described above, the influence of theheight difference of the feet 58 is about half the influence of theheight difference of the knees, so that Kxfootdiff and Kthfootdiff areone half of Kxkneediff and Kthkneediff.Xbfootdiffmdfd=Kxfootdiff*Zfootdiffmdfdθbfootdiffmdfd=Kthfootdiff*Zfootdiffmdfd  Expression 51

Incidentally, in the present embodiment, the body posture is correctedby rotating the body 53 about its trunk axis on the basis of thecorrection amount of the height difference between the right and leftfeet 58 and 58, so that the body position correction amount for footheight difference Xbfootdiffmdfd may be zero.

Subsequently, in the same manner as described above, based on thecorrection amount of the sum of the heights of the right and left kneesZsummdfd, the body position correction amount for knee height sumXbsummdfd and the body posture correction amount for knee height sumθbsummdfd for preventing the correction amount of the sum of the heightsof the right and left knees Zsummdfd from causing a twist or slippagebetween ground contact portions (the knees and the feet) and a floor aredetermined by geometric calculation on the basis of the correctionamount of the sum of the heights of the right and left knees Zsummdfdand the desired posture of the robot 51 at that instant (current time).

Alternatively, the relationship between the correction amount of the sumof the heights of the right and left knees and the correction amount ofthe body posture may be determined beforehand for some representativedesired postures, and it may be stored as a map or function so as todetermine Xbsummdfd and θbsummdfd on the basis of the map or functionand the correction amount of the sum of the heights of the right andleft knees Zsummdfd.

Alternatively, the correction amounts of body position/posture may bedetermined as follows. The height difference between the right and leftknees is converted into the 34th node compensating angle θ34 that causesthe difference, the sum of the heights of the right and left knees isconverted into the 1234th node compensating angle θ1234, and the heightdifference between the right and left feet is converted into the 12thnode compensating angle θ12 that causes the difference. Then, based onthese converted compensating angles, the body position/posturecorrection amounts may be determined by geometric calculation.Alternatively, the relationship between the converted compensatingangles and body position/posture correction amounts may be determinedbeforehand for some representative desired postures of the robot 51, andit may be stored as a map or function so as to determine bodyposition/posture correction amounts on the basis of the map or functionand the converted compensating angles.

Subsequently, the desired body position/posture are moved (rotationalmovement and parallel movement) by the body position/posture correctionamounts to determine desired body position/posture with correction.Specifically, the desired body position/posture are rotationally movedabout a trunk axis (or a predetermined axis of rotation (the axis ofrotation substantially in a vertical plane)) by the sum of the bodyposture correction amount for knee height difference and the bodyposture correction amount for foot height difference(θbkneediffmdfd+θbfootdiffmdfd), and this is further rotationally movedabout a lateral axis by the body posture correction amount for the sumof knee heights θbfootsummdfd, then this is further moved in parallel inthe longitudinal direction of the robot 51 by the sum of the bodyposition correction amount for knee height difference, the body positioncorrection amount for the sum of knee heights, and the body positioncorrection amount for foot height difference(Xbkneediffmdfd+Xbkneesummdfd+Xbfootdiffmdfd) thereby to determinedesired body position/posture with a twist correction.

Subsequently, based on the desired body position/posture with twistcorrection and desired n-th ground contact portion position/posture withdeformation compensation (n=1, 2, . . . , 6), a joint displacementcommand of the robot 51 is determined.

Thus, the processing of the inverse kinematics calculator 110 is carriedout as described above.

In other words, according to the sixth embodiment, if the degree offreedom is geometrically insufficient for the operation of correctingthe ground contact portion positions/postures of the robot 51, then thehierarchical compliance operation corrects not only the desired groundcontact portion positions/postures but also the desired bodypositions/postures such that a point of the body position (therepresentative point of the body) or the point (e.g., the overallcenter-of-gravity) of the weighted average position of a plurality ofportions, including the body (the weight in this case is desirably themass ratio of each portion), especially the horizontal position of thepoint is maintained, or the angle of a line that connects the point anda desired total floor reaction force central point (desired ZMP) ismaintained at the angle in a desired gait.

Further, in other words, the sixth embodiment is provided with a meansthat gives priority to the controllability of an actual floor reactionforce moment generated about a desired total floor reaction forcecentral point (desired ZMP) and determines at least either acompensating height or a compensating angle as the manipulated variableof the relative height or the inclination angle of a predeterminedground contact portion A (a knee in this case) or the manipulatedvariable of the relative height or the inclination angle of a nodehaving a predetermined ground contact portion A as a descendant node inorder to enhance the stability of the entire posture control of therobot 51, a means for determining the correction amount of at leasteither the posture or the position of the body while maintaining theposition of the body or the aforesaid weighted average position of aplurality of portions, including the body, at approximately the positionin a desired gait on the basis of at least either the aforesaidcompensating height or compensating angle, and an inverse kinematicscalculating means for determining a joint displacement on the basis ofat least either the position or the posture of a predetermined groundcontact portion B (foot in this case) except for the aforesaidpredetermined ground contact portion A and the body position/posturecorrected by the aforesaid correction amount.

Incidentally, in the sixth embodiment, instead of rotating a bodyposture about the trunk axis of the body 53 of the robot 51, the body 53may be rotated using the vertical axis as the axis of rotation and usingthe waist (the bottom end portion of the body 53) of the robot 51 as thecenter, as shown in FIG. 64( c). In this case, however, the deviation ofthe center-of-gravity G of the body 53 in the lateral direction wouldincrease. Alternatively, the body posture may be rotated using an axislocated midway between the trunk axis of the body 53 and the verticalaxis as the axis of rotation. Alternatively, the position and theposture of the body 53 may be simultaneously corrected on the basis ofthe correction amount of the height difference between both knees or thecorrection amount of the height difference between both feet 58 and 58.Further, instead of maintaining the position of the center-of-gravity Gof the body 53 unchanged, the position/posture of the body 53 may becorrected such that the overall center-of-gravity position of the robot51 or the position of the representative point of the body 53 remainunchanged.

In any case, if the degree of freedom is geometrically insufficient inthe operation for correcting the position/posture of a ground contactportion of the robot 51 by the compliance operation or the like, atleast either the position or the posture of the body 53 may be correctedfrom the position/posture of a desired gait so as to restrain theoccurrence of a slippage, such as a twist, of a ground contact portion.Further, instead of correcting at least either the position or theposture of the body 53, at least either the position or the posture of apredetermined portion other than the body 53 may be corrected.

Seventh Embodiment

An explanation will now be given about a control device for a mobilerobot according to a seventh embodiment of the present invention.

In the sixth embodiment, the position/posture of the body have beencorrected in order to prevent a slippage, such as a twist, of a groundcontact portion and to minimize the deviations of the position of theoverall center-of-gravity of the robot 51 and an inertial force. In thepresent embodiment (the seventh embodiment), a slippage, such as atwist, of a ground contact portion has been allowed to a certain extent,and a joint displacement (the displacement between a hip joint 55 and aknee joint 56) or the position/posture of a ground contact portion havebeen preferentially corrected so as to restrain the deviations of theposition of the overall center-of-gravity of the robot 51 and aninertial force.

More specifically, in the present embodiment, an inverse kinematicscalculator 110 carries out the processing shown by the block diagram ofFIG. 66 in place of the processing of FIG. 65 explained in the sixthembodiment. Except for this difference, the present embodiment is thesame as the sixth embodiment.

The inverse kinematics calculator 110 of the present embodiment will beexplained with reference to FIG. 66.

First, based on corrected desired n-th ground contact portionposition/posture with deformation compensation (n=1, 2, . . . , 6)(specifically, the position out of the position/posture) and desiredn-th ground contact portion position/posture (n=1, 2, . . . , 6)(specifically, the position out of the position/posture), a correctionamount of the height difference between the right and left kneesZkneediffmdfd, a correction amount of the sum of the heights of theright and left knees Zkneesummdfd, and a correction amount of the heightdifference between the right and left feet Zfootdiffmdfd are determinedaccording to the aforesaid expression 47, expression 48, and expression49, and further, the height difference between the right and left kneesis converted into a 34th node compensating angle θ34 that causes thedifference. Similarly, the sum of the heights of the right and leftknees is converted into a 1234th node compensating angle θ1234 thatcauses the sum, and the height difference between the right and leftfeet is converted into a 12th node compensating angle θ12 that causesthe difference.

Next, based on the converted node compensating angles, jointdisplacement correction amounts are determined. This is performed asfollows. First, a case is assumed where a joint displacement iscorrected while fixing the body position/posture of a desired gait andretaining the angle of a segment that connects a desired total floorreaction force central point (a desired 123456th node floor reactionforce central point (point P in FIG. 61)) and the overallcenter-of-gravity of the robot 51 (or the center-of-gravity of the bodyor the center-of-gravity of a plurality of portions, including the body53) unchanged. In this case, a j-th joint displacement correction amount(j=1, 2, . . . , total number of joints) for the inclination angle(compensating angle) of an n-th node (n=12, 34, 1234) corresponding toeach of ground contact portions (a knee and foot 58) to become a unitperturbation amount is determined, and this is defined as a sensitivityLn_j of the j-th joint displacement relative to the n-th nodecompensating angle.

More generally, the j-th joint displacement correction amount (j=1, 2, .. . , total number of joints) for the relative angle between the segmentconnecting a desired total floor reaction force central point (a desired123456th node floor reaction force central point) and an overallcenter-of-gravity of the robot 51 (or the center-of-gravity of the bodyor the center-of-gravity of a plurality of portions, including the body53) and the inclination angle of an n-th node (n=12, 34, 1234)corresponding to each of ground contact portions (a knee and the foot58) to become a unit perturbation amount is determined when a jointdisplacement of a desired gait has been corrected, and this is definedas a sensitivity Ln_j of the j-th joint displacement relative to then-th node compensating angle.

Specifically, if Ln_knee_r denotes the sensitivity of a right knee jointdisplacement relative to an n-th node compensating angle (n=12, 34,1234), Ln_knee_l denotes the sensitivity of a left knee jointdisplacement relative to the n-th node compensating angle, Ln_hip_rdenotes the sensitivity of a right hip pitch joint displacement relativeto the n-th node compensating angle, and Ln_hip_l denotes thesensitivity of a left hip pitch joint displacement relative to the n-thnode compensating angle, then each sensitivity is set according to thefollowing expression 52 in the posture of the robot 51 on its kneesshown in FIG. 58 and FIG. 59.L1234_hip_r=0, L1234_hip_l=0, L1234_knee_r=1,L1234_knee_l=1, L12_hip_r=0, L12_hip_l=0L12_knee_r=a12, L12_knee_l=−a12, L34_hip_r=a34L34_hip_l=−a34, L34_knee_r=0, L34_knee_l=0  Expression 52where a12 and a34 denote predetermined constants.

Subsequently, joint displacement correction amounts are determinedaccording to expressions 53 to 56 given below, where θknee_r denotes aright knee joint displacement correction amount, θknee_l denotes a leftknee joint displacement correction amount, θhip_r denotes a right hipjoint displacement correction amount (more specifically, the jointdisplacement correction amount in the pitch direction of the right hipjoint), and θhip_l denotes a left hip joint displacement correctionamount (more specifically, the joint displacement correction amount inthe pitch direction of the left hip joint).θknee_(—) r=L1234_knee_(—) r*θ1234+L12_knee_(—) r*θ12+L34_knee_(—)r*θ34  Expression 53θknee_(—) l=L1234_knee_(—) l*θ1234+L12_knee_(—) l*θ12+L34_knee_(—)l*θ34  Expression 54θhip_(—) r=L1234_hip_(—) r*θ1234+L12_hip_(—) r*θ12+L34_hip_(—)r*θ34  Expression 55θhip_(—) l=L1234_hip_(—) l*θ1234+L12_hip_(—) l*θ12+L34_hip_(—)l*θ34  Expression 56

Other joint displacement correction amounts are determined in the samemanner; however, it will be omitted to simplify the explanation.

For the 1234th compensating angle θ1234, the displacements (angles) ofboth knee joints 56 are corrected by the same amount as θ1234, as shownin FIG. 67. Further, for the 12th compensating angle θ12, thedisplacement (angle) of the right knee joint 56 is corrected inproportion to θ12 and also the displacement (angle) of the left kneejoint 56 is corrected at the same time by the amount obtained bymultiplying the correction amount of the displacement of the right kneejoint 56 by −1. In other words, the displacement (angle) of the rightknee joint 56 is corrected, as shown in FIG. 67, and the displacement(angle) of the left knee joint 56 is corrected in reverse therefrom.Further, for the 34th compensating angle θ34, the displacement (angle)of the right hip joint 55 in the pitch direction is corrected inproportion to θ34 and also the displacement (angle) of the left hipjoint 55 in the pitch direction is corrected at the same time by theamount obtained by multiplying the correction amount of the displacementof the right hip joint 55 in the pitch direction by −1. In other words,the displacement (angle) of the right hip joint 55 in the pitchdirection is corrected, as shown in FIG. 68, and the displacement(angle) of the left hip joint 55 in the pitch direction is corrected inreverse therefrom. Incidentally, Q1 and Q1″ in FIG. 67 denote a desiredfloor reaction force central point of the foot 58 before the correctionsof the joint displacements described above and a desired floor reactionforce central point of the foot 58 after the corrections, respectively.Further, Q3 and Q3″ in FIG. 68 denote a desired floor reaction forcecentral point of a knee before the corrections of the jointdisplacements described above and a desired floor reaction force centralpoint of the knee after the corrections, respectively.

By correcting joint displacements as described above, the relative anglebetween the segment connecting a desired total floor reaction forcecentral point (a desired 123456th node floor reaction force centralpoint) and the overall center-of-gravity of the robot 51 (or thecenter-of-gravity of the body or the center-of-gravity of a plurality ofportions, including the body 53) and the inclination angle of an n-thnode (n=12,34,1234) corresponding to ground contact portions (a knee andthe foot 58) is changed by the aforesaid converted node compensatingangles θ12, θ34, and θ1234. Thus, an actual floor reaction force isfaithfully controlled, leading to improved posture stability and groundcontact properties of the robot 51.

Instead of directly correcting joint displacement commands (desiredjoint displacements) as described above, preferentially correcteddesired ground contact portion positions/postures, which are thecorrected desired positions/postures of the ground contact portions(specifically, the feet 58 and the hands 62) whose positions/posturesare to be preferentially corrected to cause approximately the samedesired joint displacements may be determined, and then, based on thedetermined preferentially corrected desired ground contact portionpositions/postures, joint displacement commands may be determined byinverse kinematics calculation.

Because of insufficient degrees of freedom of joints, it is impossibleto determine joint displacements by the inverse kinematics calculationto satisfy desired body position/posture and all corrected desiredground contact portion positions/postures; therefore, when carrying outthe inverse kinematics calculation, some corrected desired groundcontact portion positions/postures out of all corrected desired groundcontact portion positions/postures are used. The some corrected desiredground contact portion positions/postures are referred to as theaforesaid preferentially corrected desired ground contact portionpositions/postures.

For example, in the situation shown in FIG. 67, for the 1234th nodecompensating angle θ1234, the position obtained by rotationally moving adesired foot position about a knee may be determined as thepreferentially corrected desired foot position.

Incidentally, in the sixth embodiment and the seventh embodiment, as anexample, the compliance operation in the situation wherein the robot 51is kneeling has been explained. The following will describe theadaptation to a compliance operation in a situation wherein the robot issitting on a chair. This will be explained as an eighth embodiment.

Eighth Embodiment

FIG. 69 shows the construction of an essential section of a robotaccording to the present embodiment. This robot 71 is provided withfloor reaction force sensors 73 and 73 for detecting floor reactionforces (load sensors, such as six-axis force sensors) at the right andleft, respectively, of the base end surface of buttocks 72. Instead ofproviding the floor reaction force sensors 73 and 73 at the right andleft, a single floor reaction force sensor that detects the resultantforce of the forces applied to the right and left of the base endsurface of the buttocks 72 may be provided.

In this case, the outsides of the floor reaction force sensors 73 and 73are covered with a soft member (elastic member) 74, such as a sponge, asillustrated. To enhance the accuracy of the compliance control in theground contact portions of the buttocks 72, it is desirable to shape thesurface (ground contact surface) of the soft member (elastic member) 74into a round convex surface in addition to covering them with the softmember. It is desirable to provide the member 74 such that thehorizontal positions of the detection positions of the floor reactionforce sensors 73 and 73 (the positions of the sensor main bodies) arealigned with the horizontal position of the apex of the convex surfaceof the aforesaid member 74 especially when the robot 71 is sitting on achair through the intermediary of its buttocks 72. This arrangementreduces the nonlinearity of the relationship between a correctiveoperation of the position/posture of a ground contact portion of therobot 71 and a floor reaction force, resulting in better controlperformance of the compliance control of the robot 71.

In the robot 71, legs (link mechanisms) 52 and 52 are providedextendedly from the right and left sides of the buttocks 72. Thestructures of the legs 52 and 52, including their joints, are the sameas those of, for example, the aforesaid sixth embodiment. Hence, thesame reference marks as those related to the legs 52 and 52 in the sixthembodiment will be used, and the explanations thereof will be omitted.However, in the present embodiment, the knees of the legs 52 may not beprovided with floor reaction force sensors.

Further, as shown in the figure, a torso (body) 77 is provided on theupper side of the buttocks 72, and arms 79 and 79 are providedextendedly from both sides of the upper portion of the torso 77 throughthe intermediary of shoulder joints 78 and 78. Incidentally, the arms 79may have the same structure as that in the robot 51 of the aforesaidsixth embodiment. Further, the torso 77 is connected with the buttocks72 through the intermediary of a joint 80. In this case, the joint 80 isconstructed of a torso turning joint 80 a for turning the torso 77 inthe yaw direction relative to the buttocks 72, and a torso flexing joint80 b for tilting the torso 77 in the longitudinal and lateral directionsrelative to the buttocks 72. The joints provided in the robot 71 asdescribed above are operated by actuators, which are not shown. Althoughnot shown, a control device 50, which is similar to that in theaforesaid first embodiment and the like, is installed in the buttocks 72or the torso 77.

In the present embodiment, the hierarchical structure may be set, asshown in FIG. 70, for the robot 71 sitting on a chair or the likethrough the intermediary of the buttocks 72. More specifically, a rightfoot 58, a left foot 58, the right portion of the base end surface ofthe buttocks 72 (the portion to which a right floor reaction forcesensor 73 is attached), and the left portion of the base end surface ofthe buttocks 72 (the portion to which a left floor reaction force sensor73 is attached) as ground contact portions are associated with a firstnode, a second node, a third node, and a fourth node, respectively,which are leaf nodes. Further, a 12th node having the first node and thesecond node as child nodes and a 34th node having the third node and afourth node as child nodes have been set as intermediate nodes, and a1234th node having the 12th node and the 34th node as child nodes hasbeen set as a root node. Further, according to the policy similar tothat in the aforesaid first embodiment and the like, the nodes areassociated with desired floor reaction force central points Qn (n=1, 2,3, 4, 12, 34, 1234), as illustrated. Incidentally, a desired floorreaction force central point Q1234 of the 1234th node, which is the rootnode, is identical to a desired total floor reaction force central pointP.

In the present embodiment, the control device 50 is supposed to have afunctional construction similar to that shown in the aforesaid FIG. 2.

In this case, a desired ground contact portion trajectory of a desiredmotion in a desired gait output by a gait generating device 100 in thepresent embodiment is constructed of the desired position/posturetrajectory of each foot 58 and the desired position/posture trajectoryof the buttocks 72. The body position/posture trajectory of the desiredmotion means the position/posture trajectory of the torso 77.Incidentally, the desired motion includes the position/posturetrajectory of the distal portion of each arm 79. Further, the desiredtotal floor reaction force central point P is on a virtual plane in theair rather than on an actual floor surface.

In the present embodiment, a hierarchical compliance operationdeterminer 114 has the same functional components as those of theaforesaid sixth embodiment (refer to FIG. 62). However, in the presentembodiment, a compensating total floor reaction force moment distributordetermines and outputs the node compensating floor reaction forcemoments of the intermediate nodes and the root node in the hierarchicalstructure shown in FIG. 70 and the node compensating floor reactionforce moments of the leaf nodes corresponding to the feet 58. Further, acompensating angle determiner determines and outputs the nodecompensating angles of the intermediate nodes and the root node in thehierarchical structure shown in FIG. 70 and the node compensating anglesof the leaf nodes corresponding to the feet 58. In this case, the basictechniques for determining these node compensating floor reaction forcemoments and node compensating angles may be the same as the techniquesexplained in the first to the third embodiments or the sixth embodiment.Further, a floor configuration estimator determines estimated n-th floorheight errors Zfn_estm (n=1, 2, 3, 4) by the same technique as thetechnique explained in the aforesaid third embodiment, as in the case ofthe aforesaid sixth embodiment, and also estimates ground contactportion floor inclination errors θfn (n=1, 2) of the feet 58.

Further, a robot geometric model (inverse kinematics calculator), whichis a functional component of the control device 50, corrects thepositions/postures of the ground contact portion of the buttocks 72 andthe feet 58 (ground contact portions) to restrain the occurrence of aslippage, such as a twist, of the ground contact portions and alsocorrects the position/posture of the body (torso) 77 basically on thebasis of the corrected desired ground contact portion positions/postureswith deformation compensation and the desired body position/posturementioned above by the same technique as that in the aforesaid sixthembodiment.

The processing of the control device 50 other than that explained abovemay be the same as that of the aforesaid sixth embodiment.

Several modifications related to the embodiments explained above willnow be explained.

An actual node floor reaction force that cannot be directly detected bya floor reaction force sensor may be estimated by an observer by usinganother actual ground contact portion floor reaction force directlydetected by a floor reaction force sensor, a detected value of anacceleration sensor, a detected value of a clinometer, and the like, orit may be estimated by a simplified algebraic relationship. Forinstance, the hierarchical compliance operation is a rotationalcompliance operation in which a ground contact portion is rotated abouta desired total floor reaction force central point; therefore, even whenthe compliance operation is performed, the vertical acceleration of theoverall center-of-gravity of the robot (or an acceleration component inthe direction of a segment that connects a desired total floor reactionforce central point and the overall center-of-gravity) substantiallyagrees with the vertical acceleration of the overall center-of-gravityin a desired gait (desired motion) of the robot (or an accelerationcomponent in the direction of a segment that connects a desired totalfloor reaction force central point and the overall center-of-gravity).Hence, the sum of all actual ground contact portion floor reaction forcevertical components substantially agrees with the value obtained bymultiplying the sum of the vertical acceleration of the overallcenter-of-gravity in the desired gait (desired motion) of the robot andthe gravitational acceleration by the total mass of the robot.

Thus, if an actual n-th node floor reaction force cannot be directlydetected, then first, the sum (hereinafter referred to as the actualnon-n-th node floor reaction force) of the actual floor reaction forces(actual ground contact portion floor reaction forces) of all leaf nodesthat do not have an n-th node as their ancestor node and that are notthe n-th node itself is determined.

Next, an estimated n-th node floor reaction force, which is an estimatedvalue of an actual n-th node floor reaction force, is determinedaccording to the following expression 57.Estimated n-th node floor reaction force=(Overall center-of-gravityacceleration of desired gait+Gravitational acceleration)*Totalmass−Actual non-n-th node floor reaction force  Expression 57

If the body or the like is provided with an acceleration sensor, thenthe center-of-gravity acceleration of the entire robot (hereinafterreferred to as an estimated overall center-of-gravity acceleration) maybe estimated on the basis of a detected value of the acceleration sensorand at least one of a desired gait and an actual joint displacement, andan estimated n-th node floor reaction force, which is the estimatedvalue of an actual n-th node floor reaction force, may be determinedaccording to the following expression 58.Estimated N-th Node Floor Reaction Force=(Estimated overallcenter-of-gravity acceleration+Gravitational acceleration)*Totalmass−Actual non-n-th node floor reaction forces  Expression 58

For example, in the aforesaid sixth embodiment, if no floor reactionforce sensors for detecting knee floor reaction forces are provided orfloor reaction force sensors for detecting knee floor reaction forcesfail, then an estimated 34th node floor reaction force is determinedaccording to the following expression 59.Estimated 34th node floor reaction force=(Overall center-of-gravityacceleration of desired gait+gravitational acceleration)*Totalmass−Actual non-34th node floor reaction forces=Overallcenter-of-gravity acceleration of desired gait*Total mass−(Actual 12thnode floor reaction force+Actual 56th node floor reactionforce)  Expression 59

Further, the estimated 34th node floor reaction force is used in placeof an actual 34th node floor reaction force to determine the 1234th nodecompensating angle θ1234 by the compliance operation processing, and anestimated 1234th node relative floor height error Z1234 rel_estm is alsoestimated by floor configuration estimation processing.

In determining the 34th node compensating angle θ34, it is assumed thatthe floor is as expected, and an estimated third node floor reactionforce, which is the estimated value of an actual third node floorreaction force, and an estimated fourth node floor reaction force, whichis the estimated value of an actual fourth node floor reaction force,are determined on the basis of the corrected desired third groundcontact portion position/posture with deformation compensation, thecorrected desired fourth ground contact portion position/posture withdeformation compensation, and an estimated 34th node floor reactionforce.

Further, the estimated third node floor reaction force and the estimatedfourth node floor reaction force are used in place of an actual thirdnode floor reaction force and an actual fourth node floor reaction forceto determine the 34th node compensating angle θ34 by the complianceoperation processing.

Instead of determining the corrected desired node floor reaction forcemoment about a desired node floor reaction force central point on thebasis of the required moment for restoring the posture of the robot(compensating total floor reaction force moment) so as to be equivalentto adding the required moment for restoration as described above, acorrected desired node floor reaction force central point obtained bycorrecting a desired node floor reaction force central point may bedetermined on the basis of the required moment for restoration(compensating total floor reaction force moment). In this case, adesired node floor reaction force moment about a desired node floorreaction force central point is not corrected, so that it remains to bezero.

Further, in order to compensate for a component that could not becontrolled within an n-th node floor reaction force (an expected ordetected control error component), the desired floor reaction force ofthe parent node of the n-th node may be corrected. More specifically,the value of the difference between an actual n-th node floor reactionforce and a desired n-th node floor reaction force in the last controlcycle or the value obtained by passing the aforesaid difference througha low-pass filter may be added to the desired floor reaction force ofthe n-th node.

Further, in the aforesaid embodiments, the compensating total floorreaction force moments have been generated about desired total floorreaction force central points; however, this is not essential in thepresent invention. In such a case, for example, the compensating totalfloor reaction force moment Mdmd may be set to zero in carrying out theprocessing of the embodiments described above.

In the processing of estimating a floor configuration, a tree structurethat is different from the tree structure for the compliance control maybe set. The tree structure may have two layers composed of a root nodeand leaf nodes. Further, the weight of each node may be set to bedifferent from that for the compliance control as long as the weight ofthe node becomes zero in a period during which a node floor reactionforce is zero (a period during which all ground contact portionsbelonging to the node or a ground contact portion corresponding to thenode moves in the air).

Further, in the processing of estimating a floor configuration, eachcorrected node weight may be determined on the basis of the aforesaidcorrected desired node floor reaction force central point, and a vectorhaving the determined corrected node weight as an element may be used asthe weight for estimating the floor configuration.

When determining an estimated relative floor height error of each point(each ground contact portion) in the processing of estimating a floorconfiguration, instead of using an actual floor reaction force, amechanism compliance model, and the detected value of a posture sensor,a deformation amount detector for detecting the deformation amount of acompliance mechanism or the like may be provided so as to use a detectedvalue of the deformation amount detector and a detected value of aposture sensor.

If a floor configuration is to be not only relatively estimated butglobally estimated also (estimating a floor configuration on a globalcoordinate system), then the following may be carried out.

1) Estimating the global height of a floor in a case where the height ofthe body of a robot on the global coordinate system (hereinafterreferred to as the global height) can be detected:

A detector that detects the global height of a predetermined portion,such as a body, of a robot (a body height estimator using anacceleration sensor (e.g., the estimator disclosed in PCT/JP03/05448 bythe present applicant) or an external sensor, such as a visual sensor)is used. The value of an estimated floor height error on the globalcoordinate system is determined on the basis of the estimated bodyglobal height, a joint displacement (an actual joint displacement or adesired joint displacement), the detected value of an actual bodyposture inclination, and the detected value of an actual floor reactionforce. Thus, the estimated floor height error of the root node will havea meaning as the weighted average value of the global estimated floorheight error of every ground contact portion.

2) Estimating the global height of a floor in a situation wherein theglobal height of the body is unknown:

If the global value of a certain estimated n-th floor height error (theerror in the global coordinate system rather than the relative value ina group) is known, then the global values of the estimated floor heighterrors of all points (ground contact portions) are determined on thebasis of the above known global value and estimated relative floorheight errors. If the global value of the estimated n-th floor heighterror at the start point of a travel of a robot is known, then theglobal value of the floor height error of a ground contact portion thatnewly comes in contact with the ground will be sequentially determinedas the robot continues to travel; therefore, a map of floor heights on atravel trajectory of the robot can be created by storing the abovesequentially determined global values.

Further, in the processing of estimating a floor configuration, eachcorrected node weight (obtained by correcting the value of the weight ofeach node) may be determined on the basis of the aforesaid correcteddesired node floor reaction force central point, and the determinedcorrected node weight may be used as the weight for defining an actualn-th node relative floor height or the like in estimating the floorconfiguration. Further, in estimating a floor configuration, the weightused to define the actual n-th node relative floor height or the likedoes not necessarily have to be the same as the aforesaid weightdetermined by the desired floor reaction force distributor.

Supplementally, the weight for defining the actual n-th node relativefloor height or the like does not necessarily have to agree with theweight determined by the desired floor reaction force distributor;however, if they are to agree with each other (in this case, the desiredfloor reaction force central point of the root node will automaticallyagree with a desired total floor reaction force central point), then theinfluence of a floor configuration error can be canceled simply byadding an n-th node estimated floor inclination error to an n-th nodecompensating angle (n=1, 2, . . . ) for the compliance operation, thusmaking it possible to reduce the calculation volume for canceling theinfluence of the floor configuration error.

Furthermore, estimated floor configuration errors may be stored as themap information on a floor configuration, and a desired gait may begenerated on the basis of the stored map information of the floorconfiguration when a robot walks in the same place next time.

Incidentally, in the first and the second embodiments described above,the block diagrams showing the arithmetic processing functions may besubjected to equivalent modifications by, for example, changing thearithmetic processing sequence.

Further, the present invention can be applied not only to a mobile robotbut also to a wheel-type mobile body having an active suspension.

INDUSTRIAL APPLICABILITY

As described above, the present invention is useful as the one thatmakes it possible to provide a mobile body, such as a legged mobilerobot, that permits highly stable, smooth operations to be achieved byproperly controlling floor reaction forces.

1. A control device for a mobile body comprising: a desired gaitdetermining means for determining a desired gait composed of a desiredmotion and a desired floor reaction force of a mobile body having threeor more ground contact portions connected through the intermediary of aplurality of joints such that mutual relative heights thereof can bemanipulated; a floor reaction force detecting means for detecting orestimating an actual floor reaction force acting on each of the groundcontact portions, and controls the operation of the mobile body to makea gait of the mobile body follow a desired gait, wherein the groundcontact portions are classified into a tree structure such that each ofthe ground contact portions becomes a leaf node, and an intermediatenode exists between the leaf node and a root node having all the leafnodes as descendant nodes, and when a B-th node, which is each node inthe tree structure, is the leaf node, then a floor reaction force actingon a ground contact portion that is the leaf node is defined as the nodefloor reaction force of the B-th node, and when the B-th node has childnodes, then the resultant force of the node floor reaction forces of allchild nodes of the B-th node is defined as the node floor reaction forceof the B-th node, a means for determining an actual node floor reactionforce as the actual value of the node floor reaction force of each nodefrom the detected or estimated actual floor reaction force of eachground contact portion; a means for determining a desired node floorreaction force as the desired value of a node floor reaction force ofeach node on the basis of at least a desired floor reaction force of thedesired gait; and a node operation controlling means that determines, onC-th nodes, which is a node having a plurality of ground contactportions as descendant nodes, the correction amounts of the desiredrelative heights of a plurality of ground contact portions belonging tothe C-th node on the basis of at least the relative relationship amongthe actual node floor reaction forces of each child node of the C-thnode and the relative relationship among the desired node floor reactionforces of each child node of the C-th node, determines a correcteddesired motion by adding at least a first correction of the desiredrelative heights of the plurality of ground contact portions belongingto each C-th node to a desired motion of the desired gait on the basisof a correction amount obtained by combining the determined correctionamounts of all C-th nodes, and operates the joints such that thecorrected desired motion that has been determined is satisfied.
 2. Thecontrol device for a mobile body according to claim 1, wherein a weighthas been set on each node in the tree structure, and when the B-th node,which is each node in the tree structure, is the leaf node, then theheight of a ground contact portion that is the leaf node is defined asthe height of the B-th node, and when the B-th node has child nodes,then a weighted mean value, which uses the weight, of the heights of allchild nodes of the B-th node is defined as the height of the B-th node,and wherein when the node operation controlling means determines thecorrected desired motion for only one arbitrary node among the C-thnodes by adding a first correction of the desired relative heights of aplurality of ground contact portions belonging to that one node to adesired motion of the desired gait, then a desired height of that onenode in the corrected desired motion is maintained at a desired heightin the desired motion.
 3. A control device for a mobile body comprising:a desired gait determining means for determining a desired gait composedof a desired motion and a desired floor reaction force of a mobile bodyhaving three or more ground contact portions connected through theintermediary of a plurality of joints such that mutual relative heightsthereof can be manipulated; a floor reaction force detecting means fordetecting or estimating an actual floor reaction force acting on each ofthe ground contact portions, and controls the operation of the mobilebody to make a gait of the mobile body follow a desired gait, whereinthe ground contact portions are classified into a tree structure suchthat each of the ground contact portions becomes a leaf node, and anintermediate node exists between the leaf node and a root node havingall the leaf nodes as descendant nodes, and when B-th nodes, which iseach node in the tree structure, is the leaf node, then a floor reactionforce acting on a ground contact portion that is the leaf node isdefined as the node floor reaction force of the B-th node, and when theB-th node has child nodes, then the resultant force of the node floorreaction forces of all child nodes of the B-th node is defined as thenode floor reaction force of the B-th node, the control device for themobile body relative to a predetermined C-th node that is anintermediate node having a plurality of ground contact portions asdescendant nodes, further comprises: a means for determining an actualnode floor reaction force as the actual value of a node floor reactionforce of each child node of at least the C-th node from the detected orestimated actual floor reaction force of each ground contact portion; ameans for determining a desired node floor reaction force as the desiredvalue of a node floor reaction force of each child node of at least theC-th node on the basis of at least a desired floor reaction force of thedesired gait; and a node operation controlling means that determines acorrected desired motion by adding at least a first correction of thedesired relative heights of the plurality of ground contact portionsbelonging to the C-th node to the desired motion of the desired gait onthe basis of at least a relative relationship among the actual nodefloor reaction forces of each child node of the C-th node and a relativerelationship among the desired node floor reaction forces of each childnode of the C-th node approximates the relative relationship among thedesired node floor reaction forces of each child node of the C-th node,and operates the joints such that the corrected desired motion that hasbeen determined is satisfied.
 4. The control device for a mobile bodyaccording to claim 3, wherein a weight has been set on each node in thetree structure, and when the B-th node, which is each node in the treestructure, the leaf node, then the height of a ground contact portionthat is the leaf node is defined as the height of the B-th node, andwhen the B-th node has child nodes, then the weighted mean value, whichuses said weight, of the heights of all child nodes of the B-th node isdefined as the height of the B-th node, and wherein when the nodeoperation controlling means determines, only on the C-th node, thecorrected desired motion by adding a first correction of the desiredrelative heights of a plurality of ground contact portions belonging tothe C-th node to the desired motion, then the desired height of the C-thnode in the corrected desired motion is maintained at a desired heightin the desired motion.
 5. A control device for a mobile body comprising:a desired gait determining means for determining a desired gait composedof a desired motion and a desired floor reaction force of a mobile bodyhaving three or more ground contact portions connected through theintermediary of a plurality of joints such that mutual relative heightsthereof can be manipulated; a floor reaction force detecting means fordetecting or estimating an actual floor reaction force acting on each ofthe ground contact portions, and controls the operation of the mobilebody to make a gait of the mobile body follow a desired gait, whereinthe ground contact portions are classified into a tree structure suchthat each of the ground contact portions becomes a leaf node, and anintermediate node exists between the leaf node and a root node havingall the leaf nodes as descendant nodes, and wherein when a B-th node,which is each node in the tree structure, is the leaf node, then a floorreaction force acting on a ground contact portion that is the leaf nodeis defined as the node floor reaction force of the B-th node, and whenthe B-th node has child nodes, then the resultant force of the nodefloor reaction forces of all child nodes of the B-th node is defined asthe node floor reaction force of the B-th node, and further, when thepoint at which the horizontal component or a parallel-to-floor componentof the moment of a node floor reaction force of the B-th node becomeszero is defined as the node floor reaction force central point of theB-th node, a means for determining a desired node floor reaction forcecentral point, which is the desired position of a node floor reactionforce central point of each node, on the basis of at least one of adesired motion and a desired floor reaction force of the desired gait; ameans for determining an actual node floor reaction force as the actualvalue of a node floor reaction force of each node from the detected orestimated actual floor reaction force of each ground contact portion;and a node operation controlling means that determines, on C-th nodes,which is a node having a plurality of ground contact portions asdescendant nodes, by using, as a control amount, one of a difference(Mc_act−Mc_dmd) between an actual moment (Mc_act), which is the momentcaused by the actual node floor reaction force of the C-th node to acton a desired node floor reaction force central point of the C-th node,and a desired value (Mc_dmd) of a moment that should act on the desirednode floor reaction force central point of the C-th node, a difference(Mc_f−Mc_dmd) between a moment (Mc_f), which is obtained by subtractingthe moment caused by the actual node floor reaction force of each childnode of the C-th node to act on a desired node floor reaction forcecentral point of the child node from the actual moment (Mc_act) of theC-th node, and a desired value (Mc_dmd) of a moment that should act onthe desired node floor reaction force central point of the C-th node, avalue of the difference (ΔP) between the position of the point at whichthe horizontal component or the parallel-to-floor component of themoment of the actual node floor reaction force of the C-th node becomeszero and the position of the desired node floor reaction force centralpoint of the C-th node, and a value of the difference (ΔPf) between theposition of the point at which the horizontal component or theparallel-to-floor component of the moment, which is obtained bysubtracting the moment caused by the actual node floor reaction force ofeach child node of the C-th node to act on the desired node floorreaction force central point of the child node from the moment of theactual node floor reaction force (Mc_act) of the C-th node, becomes zeroand the position of the desired node floor reaction force central pointof the C-th node, a correction amount of the desired relative heights ofa plurality of ground contact portions belonging to the C-th node, foreach C-th node, such that the control amount approximates zero on thebasis of the control amount, and also determines, on the basis of acorrection amount obtained by combining the determined correctionamounts of all C-th nodes, a corrected desired motion obtained by addingat least a first correction of the desired relative heights of theplurality of ground contact portions belonging to each G-th node to adesired motion of the desired gait, then operates the joints such thatthe determined corrected desired motion is satisfied.
 6. The controldevice for a mobile body according to claim 5, wherein a weight has beenset on each node in the tree structure, and when the B-th node, which iseach node in the tree structure, is the leaf node, then the height of aground contact portion that is the leaf node is defined as the height ofthe B-th node, and when the B-th node has child nodes, then a weightedmean value, which uses the weight, of the heights of all child nodes ofthe B-th node is defined as the height of the B-th node, and whereinwhen the node operation controlling means determines the correcteddesired motion for only one arbitrary node among C-th nodes by adding afirst correction of the desired relative heights of the plurality ofground contact portions belonging to that one node to the desiredmotion, the desired height of that one node in the corrected desiredmotion is maintained at a desired height in the desired motion.
 7. Thecontrol device for a mobile body according to claim 6, wherein the nodeoperation controlling means determines a corrected desired motion, whichis obtained by adding a first correction of the desired relative heightsof a plurality of ground contact portions belonging to the C-th node tothe desired motion, by manipulating the mutual relative heights of thechild nodes of the C-th node on the basis of the movement amount of thedesired node floor reaction force central point of each child node ofthe C-th node, the movement amount being obtained when the desired nodefloor reaction force central point of each child node of the C-th nodeis rotated about the desired node floor reaction force central point ofthe C-th node by the rotational amount determined on the basis of atleast the control amount.
 8. The control device for a mobile bodyaccording to claim 5, wherein the node operation controlling meansdetermines, for each of the C-th nodes, the correction amounts of thedesired relative heights of the plurality of ground contact portionsbelonging to the C-th node on the basis of a movement amount of adesired node floor reaction force central point of each child node ofthe C-th node, the movement amount being obtained when the desired nodefloor reaction force central point of each child node of the C-th nodeis rotated about the desired node floor reaction force central point ofthe C-th node by the rotational amount determined on the basis of atleast the control amount.
 9. The control device for a mobile bodyaccording to claim 1 or 5, wherein each of the ground contact portionsis connected to a base body of the mobile body such that they aremovable with respect to the base body, and when determining thecorrected desired motion, the node operation controlling meansdetermines the corrected desired motion by further adding, to thedesired motion, a correction of a desired posture of the base body torestrain slippages of each of the ground contact portions belonging tothe C-th node on a floor surface.
 10. The control device for a mobilebody according to claim 9, wherein the node operation controlling meansdetermines the corrected desired motion such that the direction of thesegment that connects the desired overall floor reaction force centralpoint of the mobile body, which depends on a desired floor reactionforce of the desired gait, and the overall center-of-gravity of themobile body or a predetermined representative point of the base body ofthe mobile body in the corrected desired motion is substantiallyidentical to the direction of the segment in the desired gait.
 11. Thecontrol device for a mobile body according to claim 9, wherein the nodeoperation controlling means determines the corrected desired motion suchthat the horizontal position of the overall center-of-gravity of themobile body or the horizontal position of a predetermined representativepoint of the base body of the mobile body in the corrected desiredmotion is substantially identical to the horizontal position in thedesired motion.
 12. The control device for a mobile body according toclaim 9, wherein the mobile body comprises a plurality of linkmechanisms extended from its base body, and at least one link mechanismamong the plurality of link mechanisms is provided with a joint at leastin an intermediate portion thereof between a distal end thereof and anend thereof adjacent to the base body, and the intermediate portion isthe ground contact portion.
 13. The control device for a mobile bodyaccording to claim 12, wherein the intermediate portion is provided withan elastic member that resiliently deforms when coming into contact withthe ground.
 14. The control device for a mobile body according to claim9, wherein the mobile body comprises a plurality of link mechanismsextended from its base body, each of which has one or more joints, atleast one link mechanism among the link mechanisms and the base body areprovided with ground contact portions, and ground contact portions thatbelong to at least one of the C-th nodes include at least a groundcontact portion of the base body and ground contact portions of one ormore link mechanisms among the plurality of link mechanisms.
 15. Thecontrol device for a mobile body according to claim 14, wherein a groundcontact portion of the base body is provided with an elastic member thatresiliently deforms when coming into contact with the ground.
 16. Acontrol device for a mobile body comprising: a desired gait determiningmeans for determining a desired gait composed of a desired motion and adesired floor reaction force of a mobile body having three or moreground contact portions connected through the intermediary of aplurality of joints such that mutual relative heights thereof can bemanipulated, and a floor reaction force detecting means for detecting orestimating an actual floor reaction force acting on each of the groundcontact portions, and controls the operation of the mobile body to makea gait of the mobile body follow a desired gait, wherein the groundcontact portions are classified into a tree structure such that each ofthe ground contact portions becomes a leaf node, and an intermediatenode exists between the leaf node and a root node having all the leafnodes as descendant nodes, and wherein when B-th nodes, which is eachnode in the tree structure, is the leaf node, then a floor reactionforce acting on a ground contact portion that is the leaf node isdefined as the node floor reaction force of the B-th node, and when theB-th node has child nodes, then the resultant force of the node floorreaction forces of all child nodes of the B-th node is defined as thenode floor reaction force of the B-th node, and further, when the pointat which the horizontal component or a parallel-to-floor component ofthe moment of the node floor reaction force of the B-th node becomeszero is defined as the node floor reaction force central point of theB-th node, the control device for the mobile body, relative to apredetermined C-th node that is an intermediate node having a pluralityof ground contact portions as descendant nodes, further comprising: ameans for determining a desired node floor reaction force central point,which is the desired position of a node floor reaction force centralpoint of at least the C-th node, on the basis of at least the desiredgait; a means for determining an actual node floor reaction force as theactual value of a node floor reaction force of at least the C-th nodefrom the detected or estimated actual floor reaction force of eachground contact portion; and a node operation controlling means thatdetermines, by using, as a control amount, one of a difference(Mc_act−Mc_dmd) between an actual moment (Mc_act), which is the momentcaused by the actual node floor reaction force of the C-th node to acton a desired node floor reaction force central point of the C-th node,and a desired value (Mc_dmd) of a moment that should act on the desirednode floor reaction force central point of the C-th node, a difference(Mc_f−Mc_dmd) between a moment (Mc_f), which is obtained by subtractingthe moment caused by the actual node floor reaction force of each childnode of the C-th node to act on a desired node floor reaction forcecentral point of the child node from the actual moment (Mc_act) of theC-th node, and a desired value (Mc_dmd) of a moment that should act onthe desired node floor reaction force central point of the C-th node, avalue of the difference (ΔP) between the position of the point at whichthe horizontal component or the parallel-to-floor component of themoment of the actual node floor reaction force of the C-th node becomeszero and the position of the desired node floor reaction force centralpoint of the C-th node, and a value of the difference (ΔPf) between theposition of the point at which the horizontal component or theparallel-to-floor component of the moment, which is obtained bysubtracting the moment caused by the actual node floor reaction force ofeach child node of the C-th node to act on the desired node floorreaction force central point of the child node from the moment of theactual node floor reaction force (Mc_act) of the C-th node, becomes zeroand the position of the desired node floor reaction force central pointof the C-th node, a corrected desired motion by adding at least a firstcorrection of the desired relative heights of a plurality of groundcontact portions belonging to the C-th node to the desired motion of thedesired gait such that the control amount approximates zero on the basisof the control amount, and operates the joints such that the determinedcorrected desired motion is satisfied.
 17. The control device for amobile body according to claim 16, wherein a weight has been set on eachnode in the tree structure, and when the B-th node, which is each nodein the tree structure, is the leaf node, then the height of a groundcontact portion that is the leaf node is defined as the height of theB-th node, and when the B-th node has child nodes, then the weightedmean value, which uses said weight, of the heights of all child nodes ofthe B-th node is defined as the height of the B-th node, and whereinwhen the node operation controlling means determines, only on the C-thnode, the corrected desired motion by adding a first correction of thedesired relative heights of a plurality of ground contact portionsbelonging to the C-th node to the desired motion, the desired height ofthe C-th node in the corrected desired motion is maintained at thedesired height in the desired motion.
 18. The control device for amobile body according to claim 6 or 17, wherein the means fordetermining a desired node floor reaction force central point determinesthe desired node floor reaction force central points and weights of theC-th node and each of the descendant nodes thereof such that theweighted mean value, which uses the weight, of the positions of thedesired node floor reaction force central points of each child node ofthe C-th node will be the desired node floor reaction force centralpoint of the C-th node.
 19. The control device for a mobile bodyaccording to any one of claims 2, 4, 6, and 17, wherein on each nodehaving child nodes in the tree structure, the weights of all child nodesof each node are set such that the total sum of the weights of all thechild nodes becomes one.
 20. The control device for a mobile bodyaccording to any one of claims 2, 4, 6 and 17, further comprising ameans for variably setting the weight of the each node, wherein when theleaf node corresponding to each ground contact portion not in contactwith the ground is defined as a D-th node, then the means for settingweights sets the weight of each D-th node or the weight of at least onenode of the ancestor nodes of the D-th node to zero.
 21. The controldevice for a mobile body according to any one of claims 2, 4, 6 and 17,each of the ground contact portions being connected to a base body ofthe mobile body through the intermediary of connecting mechanisms suchthat they are movable with respect to the base body, the control devicecomprising: a desired node floor reaction force distributing means thatdetermines, on each of E-th nodes, which is a node having a parent nodein the tree structure, the translational force component of a desirednode floor reaction force of the E-th node on the basis of at least thedesired floor reaction force of the desired gait such that thetranslational force component of a desired node floor reaction force,which is the desired value of the E-th node floor reaction force, takesa value obtained by multiplying the translational force component of adesired node floor reaction force of the parent node of the E-th node bythe weight of the E-th node, wherein the node operation controllingmeans comprises a means for estimating, on the basis of at least thetranslational force component of a desired node floor reaction force ofeach leaf node belonging to the C-th node, the amount of a deformationthat occurs in a connecting mechanism between the base body and eachground contact portion belonging to the C-th node and in the groundcontact portion when the translational force component of the desirednode floor reaction force acts on each of the ground contact portionbelonging to the C-th node, and when determining the corrected desiredmotion, the node operation controlling means determines the correcteddesired motion by further adding, to the desired motion, a secondcorrection of the desired height of each of a plurality of groundcontact portions belonging to the C-th node to cancel the estimateddeformation amount.
 22. The control device for a mobile body accordingto any one of claims 2, 4, 6, and 17, each of the ground contactportions being connected to a base body of the mobile body through theintermediary of connecting mechanisms such that they are movable withrespect to the base body, the control device further comprising: anactual posture inclination detecting means for detecting or estimatingthe actual posture inclination of the base body of the mobile body; ameans for correcting the weight of each descendant node of at least theC-th node on the basis of the posture inclination error, which is thedifference between the actual posture inclination and the desiredposture of the base body in the desired motion, such that a postureinclination error approximates zero; and a desired node floor reactionforce distributing means that determines at least the translationalforce component of a desired node floor reaction force, which is thedesired value of a node floor reaction force of each descendant node ofthe C-th node, out of the desired node floor reaction force, which isthe desired value of the node floor reaction force of each node, on thebasis of at least the desired floor reaction force of the desired gait,so as to take a value obtained by multiplying the translational forcecomponent of a desired node floor reaction force of a parent node ofeach descendant node by the corrected weight of the descendant node,wherein the node operation controlling means comprises a means forestimating, on the basis of at least the translational force componentof a desired node floor reaction force of each leaf node belonging tothe C-th node, the amount of a deformation that occurs in a connectingmechanism between the base body and each ground contact portionbelonging to the C-th node and in the ground contact portion when thetranslational force component of the desired node floor reaction forceacts on each ground contact portion belonging to the C-th node, and whendetermining the corrected desired motion, the node operation controllingmeans determines the corrected desired motion by further adding, to thedesired motion, a second correction of the desired height of each of aplurality of ground contact portions belonging to the C-th node tocancel the estimated deformation amount.
 23. The control device for amobile body according to claim 16, wherein the node operationcontrolling means determines the corrected desired motion by adding afirst correction of the desired relative heights of the plurality ofground contact portions belonging to the C-th node to the desired motionon the basis of the movement amount of the desired node floor reactionforce central point of each child node of the C-th node, the movementamount being obtained when the desired node floor reaction force centralpoint of each child node of the C-th node is rotated about the desirednode floor reaction force central point of the C-th node by therotational amount determined on the basis of at least the controlamount.
 24. The control device for a mobile body according to any one ofclaims 1, 3, 5 and 16, further comprising an actual posture inclinationdetecting means for detecting or estimating the actual postureinclination of a base body of the mobile body, wherein, when determiningthe corrected desired motion, the node operation controlling meansdetermines the corrected desired motion by further adding, to thedesired motion, a correction of the desired relative heights of aplurality of ground contact portions belonging to the C-th node on thebasis of a difference between the actual posture inclination and thedesired posture inclination of the base body in a desired motion of thedesired gait such that the difference approximates zero.
 25. The controldevice for a mobile body according to claim 5 or 16, further comprisingan actual posture inclination detecting means for detecting orestimating the actual posture inclination of a base body of the mobilebody, wherein the node operation controlling means comprises a means forcorrecting at least either the desired value of the moment that shouldact on the desired node floor reaction force central point of the C-thnode or the desired node floor reaction force central point of the C-thnode on the basis of a posture inclination error, which is thedifference between the actual posture inclination and the desiredposture inclination of the base body in the desired motion, such thatthe posture inclination error approximates zero, and determines thecontrol amount by using the desired value of a moment or the desirednode floor reaction force central point, which have been corrected, inplace of the desired value of a moment or a desired node floor reactionforce central point before the correction.
 26. The control device for amobile body according to claim 5 or 16, each of the ground contactportions being connected to a base body of the mobile body through theintermediary of connecting mechanisms such that they are movable withrespect to the base body, the control device comprising: a desired nodefloor reaction force distributing means for determining, on each of E-thnodes, which is a node having child nodes in the tree structure, thetranslational force component of a desired node floor reaction force ofeach leaf node belonging to the E-th node on the basis of at least thedesired floor reaction force of the desired gait such that thehorizontal component or the parallel-to-floor component of a momentcaused by a desired node floor reaction force, which is the desiredvalue of the E-th node floor reaction force, to act on the desired nodefloor reaction force central point of the E-th node becomes zero,wherein the node operation controlling means comprises a means forestimating, on the basis of at least the translational force componentof a desired node floor reaction force of each leaf node belonging tothe C-th node, the amount of a deformation that occurs in a connectingmechanism between the base body and each ground contact portionbelonging to the C-th node and in the ground contact portion when thetranslational force component of the desired node floor reaction forceacts on each of the ground contact portion belonging to the C-th node,and when determining the corrected desired motion, the node operationcontrolling means determines the corrected desired motion by furtheradding, to the desired motion, a second correction of the desired heightof each of a plurality of ground contact portions belonging to the C-thnode to cancel the estimated deformation amount.
 27. The control devicefor a mobile body according to claim 5 or 16, each of the ground contactportions being connected to a base body of the mobile body through theintermediary of connecting mechanisms such that they are movable withrespect to the base body, the control device further comprising: anactual posture inclination detecting means for detecting or estimatingthe actual posture inclination of a base body of the mobile body; ameans for correcting the desired node floor reaction force central pointof at least the C-th node on the basis of a posture inclination error,which is the difference between the actual posture inclination and thedesired posture inclination of the base body in the desired motion, suchthat the posture inclination error approximates zero; and a desired nodefloor reaction force distributing means that determines at least thetranslational force component of a desired node floor reaction force,which is the desired value of the node floor reaction force of eachdescendant node of the C-th node, out of the desired node floor reactionforce, which is the desired value of the node floor reaction force ofeach node, on the basis of at least the desired floor reaction force ofthe desired gait such that the horizontal component or theparallel-to-floor component of the moment of a desired node floorreaction force acting on the corrected desired node floor reaction forcecentral point of the C-th node becomes zero, wherein the node operationcontrolling means comprises a means for estimating, on the basis of atleast the translational force component of a desired node floor reactionforce of each leaf node belonging to the C-th node, the amount of adeformation that occurs in a connecting mechanism between the base bodyand each ground contact portion belonging to the C-th node and in theground contact portion when the translational force component of thedesired node floor reaction force acts on each of the ground contactportion belonging to the C-th node, and when determining the correcteddesired motion, the node operation controlling means determines thecontrol amount by using the corrected desired node floor reaction forcecentral point of the C-th node in place of the desired node floorreaction force central point before the correction and also determinesthe corrected desired motion by further adding, to the desired motion, asecond correction of the desired height of each of a plurality of groundcontact portions belonging to the C-th node to cancel the estimateddeformation amount.
 28. The control device for a mobile body accordingto claim 5 or 16, each of the ground contact portions being connected toa base body of the mobile body through the intermediary of connectingmechanisms such that they are movable with respect to the base body, thecontrol device comprising: an actual posture inclination detecting meansfor detecting or estimating an actual posture inclination of a base bodyof the mobile body; a means for determining the desired value of amoment that should act on the desired floor reaction force central pointof at least the C-th node on the basis of a posture inclination error,which is the difference between the actual posture inclination and thedesired posture inclination of the base body in the desired motion, suchthat the posture inclination error approximates zero; and a desired nodefloor reaction force distributing means that determines at least thetranslational force component of a desired node floor reaction force,which is the desired value of a node floor reaction force of eachdescendant node of the C-th node, out of the desired node floor reactionforce, which is the desired value of a node floor reaction force of eachnode, on the basis of at least a floor reaction force of the desiredgait such that the moment of a desired node floor reaction force actingon the desired node floor reaction force central point of the C-th nodebecomes the determined desired value, wherein the node operationcontrolling means comprises a means for estimating, on the basis of atleast the translational force component of a desired node floor reactionforce of each leaf node belonging to the C-th node, the amount of adeformation that occurs in a connecting mechanism between the base bodyand each ground contact portion belonging to the C-th node and in theground contact portion when the translational force component of thedesired node floor reaction force acts on each ground contact portionbelonging to the C-th node, and when determining the corrected desiredmotion, the node operation controlling means determines the controlamount by using the determined desired value of the moment of the C-thnode and also determines the corrected desired motion by further adding,to the desired motion, a second correction of the desired height of eachof a plurality of ground contact portions belonging to the C-th node tocancel the estimated deformation amount.
 29. The control device for amobile body according to claim 3 or 16, wherein each of the groundcontact portions is connected to a base body of the mobile body suchthat they are movable with respect to the base body, and whendetermining the corrected desired motion, the node operation controllingmeans determines the corrected desired motion by further adding, to thedesired motion, a correction of a desired posture of the base body torestrain slippages of each of the ground contact portions belonging tothe C-th node on a floor surface.
 30. The control device for a mobilebody according to claim 29, wherein the node operation controlling meansdetermines the corrected desired motion such that the direction of thesegment that connects the desired overall floor reaction force centralpoint of the mobile body, which depends on a desired floor reactionforce of the desired gait, and the overall center-of-gravity of themobile body or a predetermined representative point of the base body ofthe mobile body in the corrected desired motion is substantiallyidentical to the direction of the segment in the desired gait.
 31. Thecontrol device for a mobile body according to claim 29, wherein the nodeoperation controlling means determines the corrected desired motion suchthat the horizontal position of the overall center-of-gravity of themobile body or the horizontal position of a predetermined representativepoint of the base body of the mobile body in the corrected desiredmotion is substantially identical to the horizontal position in thedesired motion.
 32. The control device for a mobile body according toclaim 29, wherein the mobile body is equipped with a plurality of linkmechanisms extended from its base body, and at least one link mechanismamong the plurality of link mechanisms is provided with a joint at leastin an intermediate portion between its distal end and its end adjacentto the base body, and the intermediate portion is a ground contactportion belonging to the C-th node.
 33. The control device for a mobilebody according to claim 32, wherein the intermediate portion is providedwith an elastic member that resiliently deforms when coming into contactwith the ground.
 34. The control device for a mobile body according toclaim 29, wherein the mobile body is provided with a plurality of linkmechanisms extended from its base body, each of which has one or morejoints, at least one link mechanism among the link mechanisms and thebase body are provided with ground contact portions, and ground contactportions that belong to the C-th node include at least a ground contactportion of the base body and ground contact portions of one or more linkmechanisms among the plurality of link mechanisms.
 35. The controldevice for a mobile body according to claim 34, wherein a ground contactportion of the base body is provided with an elastic member thatresiliently deforms when coming into contact with the ground.
 36. Thecontrol device for a mobile body according to any one of claims 1, 3, 5,and 16, wherein a weight has been set on each node in the treestructure, wherein when a predetermined type of state amount isassociated with each leaf node in the tree structure, the state amountof each node having child nodes is defined as a weighted mean value,which uses the weight, of the state amounts of all child nodes of thenode, the value obtained by subtracting a state amount of a parent nodeof the node from the state amount of the node is determined as a noderelative state amount of the node on each node except a root node, andzero is determined as a relative state amount of the root node, isdefined as the processing for hierarchically relativizing thepredetermined type of state amount on each node, wherein a node relativefloor reaction force Fn_rel(n=1,2, . . . ) hierarchically relativized oneach node is determined on the basis of a floor reaction force Fn(n=1,2,. . . ) acting on each of the ground contact portions, which are theleaf nodes, and when a vector (Fa1_rel, Fa2_rel, . . . , Far_rel)having, as its elements, the node relative floor reaction forces of allchild nodes aj(j=1,2, . . . , r. r denotes the total number of the childnodes of an n-th node) of an n-th node, which is an arbitrary nodehaving child nodes, is represented by a linear linkage of a plurality ofpredetermined mutually independent vectors R(j)(j=1,2, . . . , r−1) thatare all orthogonal to a vector (Wa1, Wa2, . . . , War) having theweights of all child nodes of the n-th node as its elements, a vectorhaving a coefficient of the linear linkage as its element is defined asa node expansion floor reaction force moment Mn_exp of the n-th node,and wherein a node relative height Zn_rel(n=1,2, . . . ) hierarchicallyrelativized on each node is determined on the basis of a height Zn(n=1,2, . . . ) of each of the ground contact portions, which are theleaf nodes, and when a vector (Za1_rel, Za2_rel, . . . , Zar_rel) havingthe node relative heights of all child nodes aj(j=1,2, . . . , r. rdenotes the total number of the child nodes of an n-th node) of the n-thnode as its elements is expressed by a linear linkage of the pluralityof the predetermined mutually independent vectors R(j)(j=1,2, . . . ,r−1), a vector having a coefficient of the linear linkage as its elementis defined as a node expansion inclination angle θn of the n-th node,then, the node operation controlling means determines the correcteddesired motion by adding, to the desired motion, a first correction ofthe desired relative heights of a plurality of ground contact portionsbelonging to the C-th node by using at least the node expansion floorreaction force moment Mn_exp or the node expansion inclination angle θnof the C-th node.
 37. The control device for a mobile body according toclaim 36, further comprising: a means for determining a desired floorreaction force that should act on each ground contact portion belongingto at least the C-th node on the basis of at least the desired floorreaction force of the desired gait, wherein the node operationcontrolling means comprises a means for determining a desired nodeexpansion moment, which is a desired value of the node expansion momentof the C-th node, on the basis of the desired floor reaction force ofeach ground contact portion belonging to the C-th node, a means fordetermining an actual node expansion moment, which is an actual value ofthe node expansion moment of the C-th node on the basis of an actualfloor reaction force of each ground contact portion belonging to theC-th node, and a means for determining a manipulated variable of thenode expansion inclination angle of the C-th node on the basis of theactual node expansion moment and the desired node expansion moment, anddetermines the corrected desired motion by adding, to the desiredmotion, a first correction of the desired relative heights of aplurality of ground contact portions belonging to the C-th node on thebasis of the determined manipulated variable of the node expansioninclination angle.
 38. The control device for a mobile body according toany one of claims 1, 3, 5, and 16, further comprising: a floorconfiguration estimating means for estimating a parameter that specifiesthe relative heights of the ground contact surfaces of a plurality ofground contact portions belonging to at least the C-th node as the floorconfiguration parameter that represents a floor configuration, whereinthe node operation controlling means determines the corrected desiredmotion by further adding a third correction of the desired relativeheights of a plurality of the ground contact portions belonging to theC-th node on the basis of an estimated value of the floor configurationparameter when determining the corrected desired motion.
 39. The controldevice for a mobile body according to claim 38, further comprising ameans for determining an actual node floor reaction force, which is theactual value of a node floor reaction force of each child node of atleast the C-th node, from an actual floor reaction force of each groundcontact portion belonging to the C-th node, wherein the floorconfiguration estimating means performs estimation while sequentiallyupdating the floor configuration parameter on the basis of a past valueof an estimated value of the floor configuration parameter, a differencebetween the corrected desired motion and the desired motion, thecorrected motion, at least either a detected value or an estimated valueof an actual joint displacement, which is a displacement amount of eachjoint of the mobile body, the actual posture inclination, and a relativerelationship among the actual node floor reaction forces of each childnode of at least the C-th node.
 40. The control device for a mobile bodyaccording to claim 38, each of the ground contact portions beingconnected to the base body of the mobile body through the intermediaryof connecting mechanisms such that they are movable with respect to thebase body, the control device comprising: a deformation amount detectingmeans for detecting or estimating the amounts of deformations thatoccurs in the connecting mechanisms between the base body and each ofthe ground contact portions belonging to the C-th node and in the groundcontact portions, wherein the floor configuration estimating meansperforms estimation while sequentially updating the floor configurationparameter on the basis of a past value of an estimated value of thefloor configuration parameter, a difference between the correcteddesired motion and the desired motion, the corrected motion, at leasteither a detected value or an estimated value of an actual jointdisplacement, which is a displacement amount of each joint of the mobilebody, the actual posture inclination, and the deformation amountsassociated with each of the ground contact portions belonging to atleast the C-th node.
 41. The control device for a mobile body accordingto claim 40, wherein the deformation amount detecting means estimatesthe deformation amounts on the basis of the actual floor reaction forcesof each of the ground contact portions.
 42. The control device for amobile body according to claim 38, wherein a weight of each node in thetree structure has been set, and when the B-th node, which is each nodein the tree structure, is the leaf node, then the height of the groundcontact surface of a ground contact portion that is the leaf node isdefined as the height of the ground contact surface of the B-th node,and when the B-th node has child nodes, then a weighted mean value,which uses the weight, of the heights of the ground contact surfaces ofall child nodes of the B-th node is defined as the height of the groundcontact surface of the B-th node, then the floor configuration parameterestimated by the floor configuration estimating means is a parameterthat specifies the relative relationship among the heights of the groundcontact surfaces of a plurality of child nodes of the C-th node.
 43. Thecontrol device for a mobile body according to claim 42, wherein, when atleast one of the child nodes of the C-th node is likely to float, thefloor configuration estimating means estimates the floor configurationparameter that specifies the relative relationship among the heights ofthe ground contact surfaces of the plurality of child nodes, whilemaintaining the relative height of the ground contact surface of thatchild node, which is likely to float, at a fixed value.
 44. The controldevice for a mobile body according to claim 38, wherein the floorconfiguration estimating means estimates the floor configurationparameter by using a low-pass filter so as to alleviate fluctuations inthe floor configuration parameter.
 45. The control device for a mobilebody according to claim 38, wherein a weight has been set on each nodein the tree structure, and wherein when a predetermined type of stateamount is associated with each leaf node in the tree structure, thestate amount of each node having child nodes takes a weighted meanvalue, which uses the weight, of the state amounts of all child nodes ofthe node, a value obtained by subtracting a state amount of a parentnode of each node from the state amount of the node is determined as anode relative state amount of the node on each node except a root node,and zero is determined as a relative state amount of the root node, isdefined as the processing for hierarchically relativizing thepredetermined type of state amount on each node, wherein when a noderelative floor reaction force Fn_rel(n=1,2, . . . ) hierarchicallyrelativized on each node is determined on the basis of a floor reactionforce Fn(n=1,2, . . . ) acting on each of the ground contact portions,which are the leaf nodes, and when a vector (Fa1_rel, Fa2_rel, . . . ,Far_rel) having, as its elements, the node relative floor reactionforces of all child nodes aj(j=1,2, . . . , r. r denotes the totalnumber of the child nodes of an n-th node) of an n-th node, which is anarbitrary node having child nodes, is represented by a linear linkage ofa plurality of predetermined mutually independent vectors R(j)(j=1,2, .. . , r−1) that are all orthogonal to a vector (Wa1,Wa2, . . . , War)having the weights of all child nodes of the n-th node as its elements,then a vector having a coefficient of the linear linkage as its elementis defined as a node expansion floor reaction force moment Mn_exp of then-th node, wherein when a node relative height Zn_rel(n=1,2, . . . )hierarchically relativized on each node is determined on the basis of aheight Zn(n=1,2, . . . ) of the ground contact surface of each of theground contact portions, which are the leaf nodes, when a vector(Za1_rel, Za2_rel, . . . , Zar_rel) having the node relative heights ofall child nodes aj(j=1,2, . . . , r. r denotes the total number of thechild nodes of an n-th node) of the n-th node as its elements isexpressed by a linear linkage of the plurality of the predeterminedmutually independent vectors R(j)(j=1,2, . . . , r−1), then a vectorhaving a coefficient of the linear linkage as its element is defined asa node expansion inclination angle θn of the n-th node, and wherein thefloor configuration estimating means estimates the floor configurationparameter by using at least the node expansion floor reaction forcemoment Mn_exp or the node expansion inclination angle θn of the C-thnode.
 46. The control device for a mobile body according to claim 45,wherein when the B-th node, which is each node in the tree structure, isthe leaf node, then the height of the ground contact surface of theground contact portion that is the leaf node is defined as the height ofthe ground contact surface of the B-th node, and when the B-th node haschild nodes, then a weighted mean value, which uses the weight, of theheights of the ground contact surfaces of all child nodes of the B-thnode, is defined as the height of the ground contact surface of the B-thnode, and wherein the floor configuration parameter includes a parameterthat uses the node expansion inclination angle to indicate the relativeheight of the ground contact surface of each child node of the C-thnode.
 47. The control device for a mobile body according to claim 45,further comprising a means for determining a desired floor reactionforce that should act on each ground contact portion belonging to atleast the C-th node on the basis of at least the desired floor reactionforce of the desired gait, wherein the node operation controlling meanscomprises a means for determining a desired node expansion moment, whichis a desired value of the node expansion moment of the C-th node, on thebasis of the desired floor reaction force of each ground contact portionbelonging to the C-th node, a means for determining an actual nodeexpansion moment, which is the actual value of the node expansion momentof the C-th node on the basis of the actual floor reaction force of eachground contact portion belonging to the C-th node, and a means fordetermining a manipulated variable of the node expansion inclinationangle of the C-th node on the basis of the actual node expansion momentand the desired node expansion moment, and determines the correcteddesired motion by adding, to the desired motion, a first correction ofthe desired relative heights of a plurality of ground contact portionsbelonging to the C-th node on the basis of the determined manipulatedvariable of the node expansion inclination angle.